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ABSTRACT 


The  thesis  proposes  a  real-time  control  algorithm  for  the  cooperation  of  a  joint  team 
consisting  of  a  Quadrotor  and  a  Ground  robot  for  coordinated  ISR  missions.  The  intended 
application  focuses  on  indoor  environments,  where  Global  Positioning  System  signals  are 
unreliable  or  simply  unavailable  so  that  the  control  algorithms  must  rely  on  local  sensor 
information.  The  thesis  describes  the  appropriate  set  up  of  the  lab  and  includes 
simulations  using  a  full  dynamic  model  of  the  quadrotor  and  robot,  demonstrating  the 
suitability  of  the  implemented  and  the  proposed  control  scheme  into  a  waypoint 
navigation  scenario. 

The  implemented  controller  uses  the  Linear  Quadratic  Regulator  method  imposed 
into  five  different  channels;  pitch,  roll,  yaw,  x-y  position  and  height,  configured  to  the 
appropriate  gains  for  smoother  following  of  the  trajectory.  The  proposed  control  scheme 
incorporates  three  key  aspects  of  autonomy;  trajectory  planning,  trajectory  following  and 
collaboration  of  the  two  vehicles.  Using  the  differentially-flat  dynamics  property  of  the 
system,  the  trajectory  optimization  is  posed  as  a  non-linear  constrained  optimization 
within  the  output  space  in  the  virtual  domain,  not  explicitly  related  to  the  time  domain.  A 
suitable  parameterization  using  a  virtual  argument  as  opposed  to  time  is  applied,  which 
ensures  initial  and  terminal  constraint  satisfaction.  The  speed  profile  is  optimized 
independently,  followed  by  the  mapping  to  the  time  domain  achieved  using  a  speed 
factor. 
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I.  INTRODUCTION 


A.  GENERAL 

An  Unmanned  Vehicle  (UV)  is  a  power  driven  vehicle  that  can  be  autonomously 
or  remotely  operated  and  controlled.  Normally,  it  can  be  deployed  or  recovered 
repeatedly  with  various  types  of  payload.  Umnanned  Vehicles  are  often  divided  in  three 
major  categories,  (1)  Air  (Unmanned  Air  Vehicles-UAVs),  (2)  Ground  (Unmanned 
Ground  Vehicles-UGVs)  and  (3)  Maritime  (Unmanned  Maritime  Vehicles-  UMVs).  The 
Maritime  category  can  be  divided  into  Surface  (Unmanned  Surface  Vehicles-USVs)  and 
Underwater  (Unmanned  Underwater  Vehicles-UUV)  functional  areas.  There  are  more 
divisions  depending  on  whether  the  UVs  are  military  or  commercial  oriented.  If  military, 
the  branch  and  missions  dictate  the  functional  role,  of  which,  Intelligence,  Surveillance 
and  Reconnaissance  (ISR)  missions  are  the  most  common.  Many  UVs  are  also  used  as 
force-multipliers.  First  wave  strikes,  aerial  refueling,  Maritime  Interdiction  Operations 
(MIO),  autonomous  payload  recovery,  search  and  rescue  operations  and  medical 
purposes  come  to  mind. 

Autonomous  cooperation  of  multiple  unmanned  vehicles  is  a  major  concern  for 
several  of  those  functional  roles.  Various  advantages  are  observed  concerning  different 
types  of  vehicles  and  missions.  For  example,  two  or  more  UAVs  cooperating  together  in 
order  to  identify  a  moving  target  on  the  ground;  UAVs  and  ground  robots  conducting 
ISR,  with  the  UAV  as  a  leader  and  the  robot  assisting  as  a  follower  or  inversely. 

The  dynamic  modeling  and  control  of  these  vehicles  is  the  focus  of  this  thesis. 
Rigorous  analysis  of  the  dynamics  and  kinematics  has  been  done  in  order  to  set  up  the 
models  for  designing  the  controllers.  The  model  of  the  ground  robot  is  extracted  from  the 
forward  and  inverse  kinematics  of  the  vehicle.  Using  multiple  sensors,  there  was  an  effort 
to  apply  several  techniques  for  the  control  of  the  UVs.  The  optimization  methods  include 
the  Linear  Quadratic  Regulator  (LQR)  and  the  use  of  differential  flatness  property  of  the 
vehicle’s  dynamics,  for  the  trajectory  planning  and  optimization  in  the  output  space. 
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In  real  applications,  like  the  dynamic  environment  of  use  ISR  missions,  quasi- 
optimal  trajectories  proved  to  be  more  effective,  by  achieving  full  autonomous  control 
with  trajectory  planning  and  path  following. 

B.  MISSIONS 

There  are  many  recent  developments  that  concern  unmanned  systems.  Most  of 
them  are  used  for  dangerous  and  “dirty”  human  jobs.  Facts  reveal  that  a  wide  range  of 
requirements  and  capabilities  have  been  developed  and  fulfilled  toward  various  warfare 
specifications,  material  requirements  and  interoperability. 

Since  UVs  can  continuously  meet  the  operator’s  requirements  and  needs  during  or 
before  the  battle,  it  is  expected  in  the  near  future  that  the  unmanned  systems  will  be  an 
indispensable  support  element  in  a  wide  area  of  missions.  Most  of  those  listed  for  any 
category  of  UVs  is  in  the  Table  1.  It  is  quite  noticeable  that  some  types  of  mission  can  be 
conducted  by  a  certain  type  of  unmanned  vehicle  and  some  others  by  all  of  them,  within 
its  capabilities. 

It  is  often  more  effective  and  desirable  for  a  combination  of  multiple  vehicles  to 
be  used  in  a  mission.  However,  there  is  a  challenge.  How  can  this  cooperation  of  two  or 
more  vehicles  could  be  achieved  successfully  in  real-time  operations?  Also,  the  question 
of  which  vehicles  should  be  chosen  for  a  specific  operation,  especially  for  the  UAVs, 
since  there  are  a  lot  of  candidates?  Observing  the  characteristics  of  the  quadrotor 
helicopter,  it  seems  right  to  compare  it  with  some  of  the  already  developed  unmanned  air 
vehicles  in  order  to  prove  how  useful  it  can  be  toward  the  ISR  missions  and  show  the 
advantages  of  our  choice. 
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Reconnaissance 

Reconnaissance 

ISR 

Precision  Target  Location  and 

Designation 

Precision  Target  Location  and 

Designation 

Communication  / 

Navigation  Network 

Node 

Signals  Intelligence 

CBRNE  Reconnaissance 

Inspection  / 

Identification 

Special  Operation  Forces  (SOF) 

Team  Resupply 

Mine  detection  / 

Countermeasures 

Open  Ocean  Anti- 

Submarine  Warfare 

(ASW) 

Battle  Management 

Weaponization  /  Strike 

Payload  Delivery 

Communication  /  Data  Relay 

Battle  Management 

CBRNE  Reconnaissance 

CBRNE  Reconnaissance 

Communication  /  Data  Relay 

SOF  Resupply 

Combat  Search  and  Rescue 

Signals  Intelligence 

Strike 

Weaponization  /  Strike 

Covert  Sensor  Insertion 

Littoral  Surface  Warfare 

Electronic  Warfare 

Littoral  Warfare 

Mine  Countermeasures 

(MCM) 

Mine  detection  / 

Countermeasures 

Counter  CCD  Camera 

Information  Operations 

Information  Warfare 

Time  Critical  Strike 

Digital  Mapping 

Digital  Mapping 

Covert  Sensor  Insertion 

Oceanography 

Decoy  /  Pathfinder 

Decoy  Pathfinder 

GPS  Pseudolite 

Bottom  Topography 

Littoral  Undersea  Warfare 

Table  1 .  Mission  for  Unmanned  Vehicles 


C.  QUADROTOR  TOWARD  OTHER  UAVs 


a.  The  limitations  of  fixed-wing  unmanned  aircrafts,  as  far  as  ISR  missions 
are  concerned  (the  inability  to  hover  or  fly  at  low  speeds),  have  motivated  the  researchers 
to  look  at  the  use  of  Rotary-wing  unmanned  vehicles.  Additionally,  the  ability  to  hover 
over  an  assigned  position  creates  the  opportunity  to  use  various  sensors,  common  or 
experimental  ones,  attached  to  the  platform.  They  can  be  used  to  collect  data,  conducting 
surveillance  for  ISR  missions.  Moreover,  the  ability  of  the  rotary  wing  UAVs  for  vertical 
takeoffs  and  landings  can  reduce  the  launch  and  recover  footprint  and  eliminate  the  need 
of  launching  mechanisms  like  catapults  and  rails  that  is  most  common  in  any  fixed-wing 
UAVs. 

b.  However,  the  large  mechanical  complexity  of  the  helicopter  is  major 
concern.  A  complex  hub  in  the  main  rotor  is  needed  for  lift  and  for  pitch  alterations. 
Also,  the  vertical  tail  rotor  is  used  to  compensate  for  the  reaction  torque  on  the  fuselage, 
comes  from  the  main  rotor.  The  quadrotor,  however,  consists  of  a  pair  of  counter  rotating 
rotors,  so  a  tail  rotor,  is  not  only  required  but  it  offers  better  mobility  and  increases  the 
payload  capability.  The  rotor  is  also  surrounding  by  a  protective  frame,  so  additional 
safety  can  be  enhanced. 

c.  It  will  be  useful  to  compare  the  different  configurations  and  mechanisms 
in  UAVs  that  already  were  used  for  research  purposes.  Each  of  them  present  advantages 
and  drawbacks  as  far  as  maneuverability,  compactness,  mechanics  and  aerodynamics 
complexity,  payload  capability  and  survivability  is  concerned.  In  Table  2,  a  small 
comparison  between  those  different  VTOL  vehicle  concepts  is  described.  It  is  noticeable 
that  the  quadrotor  turns  out  to  be  the  best  configuration  among  them  all. 

d.  However,  there  are  some  disadvantages.  The  quadrotor  experiences 
dynamic  instabilities  and  has  higher  sensitivity  to  disturbances.  These  facts  increase 
difficulty  to  the  control  implementation.  Also,  the  extra  motors  it  carries,  gives  the  ability 
to  carry  larger  payload,  but  results  in  the  increase  of  energy  consumption. 
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Power  Cost 

2 

2 

2 

2 

1 

4 

3 

3 

Control  Cost 

1 

1 

4 

2 

3 

3 

2 

1 

Payload 

Volume 

2 

2 

4 

3 

3 

1 

2 

1 

Maneuverability 

4 

2 

2 

3 

3 

1 

3 

3 

Mechanics 

Simplicity 

1 

3 

3 

1 

4 

4 

1 

1 

Aerodynamics 

Complexity 

1 

1 

1 

1 

4 

3 

1 

1 

Low  Speed 

Flight 

4 

3 

4 

3 

4 

4 

2 

2 

High  Speed 

Flight 

2 

4 

1 

2 

3 

1 

3 

3 

Miniaturization 

2 

3 

4 

2 

3 

1 

2 

4 

Survivability 

1 

3 

3 

1 

1 

3 

2 

3 

Stationary 

Flight 

4 

4 

4 

4 

4 

3 

1 

2 

Toted 

24 

28 

32 

24 

33 

28 

22 

24 

Table  2.  VTOL  Concept  comparison  (l=Bad,  4=  Very  Good).  After  [1]. 


D.  RELATED  WORK  -  RECENT  DEVELOPMENTS 

Today,  a  lot  of  universities  are  focusing  their  research  projects  on  quadrotor 
UAVs.  At  the  same  time,  many  companies  have  designed  commercial  quadrotors  in  a 
very  efficient  and  effective  way.  Draganfly  Innovations  Inc.  in  Canada  is  one  of  the 
companies,  they  have  produced  many  great  products,  like  the  quadrotor  helicopter 
Draganflyer  X4  (Figure  1)  and  the  six-rotor  helicopter  Draganflyer  X6  and  Draganflyer 
RC  helicopters. 

The  Draganflyer  X4  is  a  stable  UAV  platform  [2]  equipped  with  multiple  sensors 
including  gyroscopes,  magnetometers,  accelerometers,  barometric  pressure  sensors  and 
wireless  video  camera.  It  is  used  by  many  universities,  such  as  the  Massachusetts 
Institute  of  Technology  (MIT)  [3],  Boeing  Research  and  Technology  [4],  Vanderbilt 
University  [5],  and  Concordia  University  in  Canada  [6]. 


Figure  1.  Draganflyer.  After  [1]. 


Draganflyer  RC  helicopters  were  used  in  developing  an  autonomous  Control 
System  (VECPAV)  shown  in  Figure  2  [4]  by  Vanderbilt  University.  They  controlled  the 
helicopter  by  receiving  and  processing  position  and  motion  data  from  a  sensor  and 
sending  various  control  commands  through  a  radio  transmitter.  As  reported  in  [7]  and  [8], 
Draganflyer  X6  helicopter  is  being  widely  used  for  police  actions,  too.  Mesa  County 
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Sheriff  is  the  first  Public  Safety  Agency  to  receive  an  FAA  Certificate  of  Authorization 
(CO A)  to  operate  the  Draganflyer  X6  helicopter  for  law  enforcement  use  in  over  a  3,300 
square  mile  area.  May  Regina’s  police  force,  investigating  a  homicide,  used  the 
Draganflyer  X6  UAV  helicopter  to  obtain  aerial  pictures  and  video  of  the  crime  scene. 


Figure  2.  Vanderbilt  University’s  Embedded  Computing  Platfonn.  After  [3]. 


Stanford  university  created  a  quadrotor  helicopter  platfonn  (Stanford  Testbed  of 
Autonomous  Rotorcraft  for  Multi-Agent  Control  II  -  STARMAC  II),  shown  in  Figure  3, 
and  applied  a  PID  controller  for  attitude/altitude  control  in  outside  environment  where 
disturbances  are  unpredicted.  The  first  efforts  were  not  successful  since  the  control  of  the 
vehicle  flight  was  not  accurate,.  Further  improvements  of  the  controller  are  already 
considered  [9]. 


Figure  3.  STARMAC  II  quad-rotor  UAV.  After  [8]. 
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In  Ecole  Polytechnique  Federate  De  Lausanne  (EPFL),  a  quadrotor  helicopter 
called  Omni-directional  Stationary  Flying  Outstretched  Robot  (OS4)  was  designed  for 
fully  autonomous  operation  using  many  different  control  methods.  Using  the  Lyapunov 
theory,  linear  controllers  like  Proportional  and  Integral  Derivative  (PID)  and  Linear 
Quadratic  Regulator  (LQR),  backstepping  and  sliding  mode  methods,  their  research 
proved  successful  by  using  Integral  backstepping  where  autonomous  hovering  with 
altitude  control  and  autonomous  take-off  and  landing  were  established  [1]. 


Figure  4.  OS4  Test  Bench.  After  [9]. 

Moreover,  Massachusetts  Institute  of  Technology  (MIT)  developed  a  Real-time 
indoor  Autonomous  Vehicle  test  Environment  (RAVEN)  shown  in  Figure  5,  consisting  of 
ground  UVs  and  UAV  and  a  motion  capture  system  for  tracking  in  order  to  conduct 
autonomously  multi-vehicle  missions.  It  was  a  successfully  developed  and  tested  system 
where  pilot’s  assigned  tasks  can  be  performed  in  real-time  by  a  single  or  multiple 
vehicles.  The  vehicles  followed  waypoints  created  through  a  trajectory  generation 
algorithm.  RAVEN  allows  an  operator  to  control  up  to  ten  UVs  simultaneously  [10]. 


F igure  5 .  RAVEN  Proj  ect.  After  [11]. 


8 


From  2007  the  Networked  Autonomous  Vehicles  Lab.  (NAVL)  of  Concordia 
University  used  different  quadrotor  UAVs  such  as  Draganflyer  X4  and  X-Qball  from 
Quanser  Innovations  INC  Company,  as  well  as  several  wheeled  robots  as  shown  in 
Figure  6.  They  used  various  control  techniques,  focusing  mostly  on  fault-tolerant 
cooperative  control  of  these  vehicles  [6]. 


Figure  6.  Concordia  NAVL  Project.  After  [5]. 


Research  has  been  done  by  Virginia  Tech  University  [11],  only  for  cooperation  of 
UGVs  whereas  team  of  mini  ground  heterogeneous  autonomous  vehicles  (MUVs)  were 
developed  for  urban  search  and  rescue  in  both  indoor  and  outdoor  environments  by 
utilizing  onboard  computers  and  multiple  sensors.  Autonomous  navigation,  search, 
tracking,  localization  and  mapping  (STLAM)  as  well  as  obstacle  avoidance  accomplished 
through  various  experiments  and  participation  in  competitions  as  MAGIC2010.  Figure  6 
depicts  the  team  of  those  UGVs  and  the  sensors  that  are  used  in  each  vehicle. 


Figure  7.  Virginia  Tech  University’s  team  of  MUVs.  After  [11]. 
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Various  Naval  Postgraduate  School  (NPS)  departments  have  conducted  research 
for  autonomous  ground  vehicles  through  the  years.  Small  Robot  Technology  (SMART) 
program  in  NPS  Physics  department  AXV  LAB  many  prototype  robotic  platforms  or 
military  applications.  One  of  all  the  platforms  being  developed,  was  called  “Bender”  and 
was  equipped  with  a  web-cam  and  several  ultrasound  sensors  so  as  to  move  to  designated 
destinations  autonomously  via  waypoint  navigation,  controlled  by  a  commercial  BL2000 
microcontroller,  which  was  programmed  by  Dynamic  C  language  [12].  Another  UGV, 
called  “AGB”  was  a  wheeled  vehicle,  created  by  MAJ  Ben  Miller  U.S.  Army,  that 
utilized  an  acoustic  and  IR  (infrared)  detectors  to  detect  motion,  obtaining  the  capability 
to  report  images  to  remote  monitoring  stations  via  a  web  camera,  in  order  to  assist  in  the 
interdiction  of  IED  placement  [12],  Figure  8  shows  these  two  vehicles. 


Figure  8.  “Bender”  (left)  and  “AGB”  (right)  NPS  Autonomous  Ground  Vehicles. 

After  [13]. 
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E.  LITERATURE  REVIEW 

Quadrotors  have  proved  to  be  quite  complex  vehicles,  but  present  many 
advantages  for  research,  including  the  improvement  of  their  control  perfonnance  for 
different  flight  conditions  and  through  different  control  techniques  and  objectives, 
described  in  the  following  reviews  [1],  [13-16], 

One  very  common  technique,  used  for  more  than  two  centuries  is  the  Proportional 
(P),  Integral  (I),  and  Derivative  (D)  (PID)  control  method,  where  the  proportional  control  is 
used  to  settle  the  output  signal  in  direct  proportion  to  the  controller  input,  the  integral  is  used 
to  eliminate  any  steady  state  error,  and  the  derivative  reduces  the  overshoot  of  the  system. 
PID  techniques  aiming  to  fault  tolerant  control  of  quadrotors  have  been  presented  in  the 
following  papers  [1],  [17-20], 

Another  technique,  the  feedback  linearization  (FL)  were  used  by  Altug  et  al.  [21] 
for  altitude  and  Euler  angles  stabilization  when  Lee  et  al.  [22]  implemented  an  output 
feedback  (OFB)  controller  with  an  observer  to  a  nonlinear  system  so  as  to  obtain  an 
estimate  for  the  vehicle’s  velocities.  Furthermore,  Tayebi  et  al.  [23]  used  a  PD2 
(proportional  and  twice  derivative)  feedback  structure  in  order  to  improve  the  transient 
performance  and  remove  the  disturbances,  caused  by  the  feedback  linearization  of  the 
model.  Benallegue  et  al.  [24]  showed  a  way  to  provide  with  insensitivity  to  the 
uncertainties  by  combining  the  FL  method  with  the  high-order  sliding  mode  observer 
which  acted  also  as  an  estimator  of  the  disturbances. 

The  stability  of  the  system  was  always  a  problem,  so  Kanellakopoulos  et  al. 
introduced  the  backstepping  method  [25]  so  as  to  hold  the  nonlinear  system  [26]  for  the 
controller  implementation.  This  technique  became  very  popular  through  its  variations. 
So,  Madani  and  Benallegue  [27]  used  the  backstepping  method  to  control  three  of  the 
outputs  and  later  combined  it  with  the  sliding  mode  control  [28]  to  solve  the  chattering 
phenomenon  successfully,  while  Mian  et  al.  [29]  stabilized  the  altitude  simply  by  adding 
an  integrator. 

Several  quadrotor  vehicles  were  controlled  with  Lyapunov  theory  [30],  [31], 
linear  quadratic  regulator  (LQR)  control  method  [32]  and  sliding  mode  control  [33]. 
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Castillo  et  al.  [31]  presented  a  controller  design  based  on  Lyapunov  analysis  using  a 
nested  saturation  algorithm  for  stabilization  of  the  altitude  and  the  yaw.  Real  time 
experiments  with  autonomous  take-off,  hovering,  and  landing.  The  dynamic  model  of  the 
quadrotor  was  obtained  with  a  Lagrange  approach  while  Bouabdallah  et  al.  [1],  [30] 
described  the  modeling  of  the  OS4  quadrotor  with  both  Langrange  and  Euler-Newton 
method  and  provided  results  for  altitude  and  attitude  control  from  all  of  the  mentioned 
techniques,  proving  that  the  integral  backstepping  method  was  indeed  more  successful 
and  efficient  for  his  model. 

However,  this  literature  mentions  control  techniques  for  test  benches  or  quadrotor 
platforms  in  a  laboratory  environment  or  outside  environment  with  limited  disturbances 
and  thus  nonnal  flight  conditions.  There  is  a  possibility  that  various  faults  could  happen 
during  flight,  like  actuators  and  sensors  outage  (zero  output),  transient  fault  and  bias 
failure  and  control  surface  defect.  This  will  reduce  the  safety  of  the  vehicle.  Thus 
nonlinear  control  schemes  should  be  considered  and  fault  tolerant  control  (FTC) 
techniques  are  to  be  proposed.  Based  on  FTC  methods  for  aircrafts  presented  in  [34],  [35] 
thirty  years  ago,  Zhang  et  al.  [5]  have  presented  various  papers  for  FTC,  with  the  most 
comprehensive  one,  from  Zhang  and  Jiang  [36]  where  they  analyzed  the  background  and 
different  control  schemes  for  fault  detection  and  diagnosis  (FDD),  presenting  also  the 
current  research  activities  and  future  challenges. 

Qi  et  al.  [37]  used  Kalman  filter  to  estimate  the  states  and  the  parameters  of  the 
fault-tolerant  control  while  Zhang  and  Jiang  [38]  introduced  a  two-stage  adaptive  Kalman 
filter  for  the  observation  of  the  potential  faults  and  use  of  the  extracted  data  for 
reconfiguration  of  the  controller. 

Many  simulation  and  experimental  work  has  been  done  in  Concordia  University  in 
Canada,  using  the  same  quadrotor  platfonn.  In  his  thesis[39],  Zhang  addressed  a  FTC 
design  technique  based  on  Fyapunov  based  nonlinear  control  techniques  in  order  to 
produce  acceptable  performance  for  potential  faults  in  the  quadrotor,  while  Bilhim,  in  his 
own  thesis[40]  described  a  gain-scheduled  PID  controller  for  fault  tolerant  control  of  the 
quad-rotor. 
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Another  concern  toward  the  control  of  the  quadrotor  is  the  ability  to  conduct 
various  tasks  in  real  time.  Yakimenko  et  al.  [41],  brought  Prof.  Taranenko’s  ideas  (direct 
method  of  calculus  of  variations  in  flight  dynamics  problems  with  constraints  on  states  and 
controls)  to  a  new  level  and  developed  algorithms  for  real-time  onboard  calculation  of  quasi- 
optimal  trajectories  [42,]  [43],  [44],  Various  tests  in  combat  vehicles  and  missiles  using  these 
algorithms  were  performed  onboard  5th-generation  aircraft  [45],  Kaminer  et  al.  [46]  used  this 
method  in  2006  to  generate  trajectories  for  landing  approach  of  UAVs  and  assure  flight 
deconfliction  at  maneuvering. 

Nowadays,  more  recent  researchers  are  focused  on  this  method,  posing  the 
trajectory  planning  as  a  nonlinear  constrained  optimization  problem  and  separately  the 
trajectory  (path)  following  as  different  problem  in  order  fully  autonomy  to  be  achieved. 
Using  the  differential  flatness  property  of  the  equations  of  motions,  this  control  method 
uses  the  optimization  in  the  computation  of  a  new  feasible  trajectory  that  meet  the 
dynamic  constrains  and  requirements  for  the  maneuver  of  the  vehicle  characterizing  by  a 
given  perfonnance  index.  In  2005  Yang  et  al.  [47]  addressed  a  time  optimal  control  of  a 
hovering  quadrotor  helicopter,  where  genetic  algorithms  were  adopted.  In  2006,  Cowling 
et  al.  [48],  proposed  a  complete  real-time  controller  for  autonomous  control  of  a 
quadrotor  UAV,  while  Bouktir  et  al.  in  2008  [49]  presented  a  simple  direct  method  for 
trajectory  planning  of  the  quadrotor  vehicle.  Finally,  the  same  year,  Hoffmann  et  al.  [50] 
in  Stanford  University,  developed  an  autonomous  vehicle  trajectory  tracking  algorithm 
for  the  STARMAC  platform. 
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F. 


THESIS  OBJECTIVES  -  MOTIVATIONS 


Judging  from  the  existed  literature,  the  researchers  have  succeeded  in  achieving  a 
quite  acceptable  perfonnance  by  applying  different  control  techniques  either  with  linear 
or  nonlinear  control  theory.  The  limited  experiments  of  real-time  optimal  techniques  with 
inverse  dynamics  for  quadrotor  vehicles  were  the  motivation  to  choose  this  quadrotor 
UAV  for  analysis  and  controller  implementation.  The  importance  of  the  cooperation  of 
two  or  more  vehicles  for  the  successful  completion  of  ISR  missions  was  another  urge  for 
this  thesis.  The  thesis  begins  with  the  analysis  of  the  dynamic  model  of  the  two  vehicles, 
a  quadrotor  and  a  ground  robot,  follows  the  derivation  of  the  equations  of  motion  in  order 
to  set  up  the  state  model  that  will  be  used  for  the  controllers.  Finally,  the  controller 
implementation  takes  place  separately  for  each  vehicle.  For  the  quadrotor,  it  addresses 
LQR  control  technique  for  the  four  controller  models  that  will  be  used  for  the  path 
following  of  the  trajectories  that  will  be  generated  from  a  trajectory  generator  in  real 
time.  The  trajectory  generator  will  use  quasi  optimal  solution  for  the  generation  of  the 
trajectories  through  parameterization  with  fifth  order  polynomials.  For  the  ground  robot 
through  forward  and  inverse  kinematics  an  LQR  controller  will  be  used  to  set  up  the 
vehicle  as  a  leader,  where  after  The  specific  objectives  are  as  follows: 

•  Derive  a  mathematical  model  of  the  quadrotor  UAV,  according  to  its 
particular  physical  structure  and  dynamics; 

•  Derive  a  mathematical  model  of  the  ground  robot,  according  to  forward 
and  inverse  kinematics; 

•  Derive  a  unified  control  architecture  that  enables  control  of  multiple 
heterogeneous  UAV/UGV  teams; 

•  Develop  a  LQR  controller  for  its  vehicle  without  fault  consideration; 

•  Design  a  Trajectory  Generator  for  the  quadrotor; 

•  Implement  the  two  models  in  Matlab  -  Simulink  integrated  with  Quarc 
software  for  the  real-time  operation  of  the  vehicles  ,  Qball-X4  quadrotor 
UAV  and  Qbot  ground  vehicle 

•  Finally,  perform  simulations  for  the  individual  control  of  the  vehicles  and 
the  cooperation  control  to  analyze  the  performance  of  the  design 
technique. 
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G.  OUTLINE 

Having  introduced  the  concept  and  the  objectives  of  the  thesis  in  the  first  chapter, 
the  second  chapter  will  address  the  setup  of  Quanser  control  lab,  consisting  of  the 
experiment  platforms  UAV  QBall  X-4  and  UGV  Qbot  and  describes  their  sensors  and 
elements  and  the  whole  set  up  needed  for  the  completion  of  the  experiment. 

Chapter  III  is  devoted  to  modeling  two  types  vehicles.  It  describes  the  quadrotor’s 
physical  structure  and  dynamics,  and  derives  its  six  degree-of-freedom  nonlinear 
mathematical  model.  The  same  procedure  is  described  for  the  ground  robot  through  its 
forward  and  inverse  kinematics  for  the  derivation  of  the  equation  of  motion. 

Chapter  IV  addresses  the  existing  and  proposed  control  architecture  within  the 
Quanser  lab. 

The  architecture  and  the  simulation  process  for  the  cooperation  of  the  two  UVs 
and  results  of  the  lab  experiments  are  shown  in  order  to  show  the  effectiveness  of  the 
thesis  work. 

Finally,  Chapter  VI  highlights  the  conclusion  and  recommendations  for  future 

work. 
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II.  QUANSER  LABORATORY  SETUP 


A.  INTRODUCTION 

The  laboratory,  where  the  experiments  was  carried  out,  is  designed  in  such  way, 
as  to  give  the  opportunity  to  any  researcher  to  perform  research  in  an  indoor,  safe 
environment  that  can  easily  controlled  and  /  or  reconfigured  depending  on  the  different 
demands.  It  is  an  open-architecture  environment  that  consists  of  Quanser’s  unmanned 
vehicles,  the  quadrotor  UAV  QBall-X4  [51]  and  the  ground  robot,  Qbot  [52],  a  single 
ground  station  and  a  localization  system  containing  ten  cameras,  required  for  tracking  the 
positions  of  vehicles  in  the  lab  space,  since  no  GPS  reception  is  available  indoors. 

Every  component  of  the  lab  can  be  operated  from  ground  station  computer,  which 
is  equipped  with  the  required  research  software,  including  MATLAB  /  Simulink  with 
real-time  control  software  QuaRC  installed  and  the  OptiTrack  Tracking  Tools  program 
for  managing  fully  integrated  with  QuaRC,  OptiTrack  camera  system.  In  addition, 
wireless  communications  are  achieved  through  a  wireless  network  adapter,  inserted  to  the 
ground  control  station  computer  while  a  USB  2.0  port  is  used  for  connecting  the 
OptiTrack  indoor  localization  system. 

The  Quanser,  embedded  avionics  data  acquisition  card,  HiQ  on  each  vehicle, 
provides  high-resolution  inertial  measurement  sensors  and  motor  outputs;  while  the 
controllers  created  from  the  operator  in  the  host  PC  can  be  downloaded  and  executed  on 
their  embedded  Gumstix  Verdex  target  computer  [53],  A  laboratory  illustration  with  a 
collage  of  various  photographs  of  the  camera  set  up  is  shown  in  Figure  9. 
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Figure  9.  Implemented  Laboratory  illustration. 


B.  QUANSER  REAL-TIME  CONTROL  (QuaRC)  SOFTWARE 


Quanser  Real-time  control  (QuaRC)  software  [52]  is  a  rapid  control  software  for 
teaching  and  research  applications,  created  in  the  paths  of  its  predecessor  WinCon,  the 
first  real-time  software  to  run  Simulink-generated  code  under  Windows.  QuaRC  is 
compatible  and  integrated  with  Simulink  and  real-time  workshop  and  gives  the 
opportunity  to  the  operator  to  design  controllers,  generate  code  and  execute  it  avoiding 

low  level  programming  and  pure  code  writing. 
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QuaRC  runs  under  Windows  XP  or  Vista  and  is  targeting  either  the  host  computer 
or  a  remote  computer  running  either  Windows  or  the  QNX  RTOS.  For  embedded 
computers  on  unmanned  vehicles,  QuaRC  runs  under  QNX  and  the  Quanser’s  computer 
Gumstix  Verdex. 

So,  one  or  multiple  controllers  designed  in  Simulink  are  able  to  be  converted  into 
real  time  code  through  QuaRC  and  run  on  different  target  processors  even  on  different 
chipsets  running  different  operating  systems.  QuaRC  also  provides  the  operators  with  the 
ability  to  tune  the  control  parameters  while  the  model  is  running,  allowing  for  rapid 
design  and  test  iterations,  with  little  to  no  recompilation  required. 

The  host  and  target  components  referred  to  as  QuaRC  Host  and  QuaRC  Target  in 
QuaRC  software  are  designed  to  communicate  even  via  Internet,  in  order  the  user  to 
download  a  controller  anywhere  and  control  it  from  a  remote  location,  tune  and  configure 
the  control  parameters  while  the  controller  runs,  plot  real-time  data  and  save  it  inside 
MATLAB. 

Low  level  programming  is  eliminated  by  utilizing  the  QuaRC  control  software 
tool,  since  the  number  of  Simulink  blocks  to  be  learned,  are  reduced.  So  the  controller 
design  becomes  the  priority  avoiding  interfacing  issues  and  extreme  code-writing. 

QuaRC  blocks/tools  are  quite  extensible  for  more  systems  and  commands  to  be 
added  and  even  allow  a  Simulink  model  to  communicate  with  third  party  devices,  while 
they  provide  the  mathematical  framework  for  controlling  the  various  devices.  The  most 
important  ones  are: 

•  Communication  blocks  for  the  supported  communication  protocols,  like 
TCP/IP,  UDP,  Serial  port  and  Shared  memory. 

•  Hardware-In-the-Loop  (HIL)  block  set,  an  extensible  hardware  in-the-loop 
API  used  to  interface  with  over  50  data  acquisition  cards. 

•  the  Vehicle  Abstraction  Layer  (VAL)  block  set,  consisting  of  a  series  of 
blocks  that  provide  a  group  of  high-level  primitive  commands  to  the 
operator,  while  the  VAL  deals  with  the  vehicle  hardware  communication. 

•  Through  the  use  of  the  Virtual  Reality  Toolbox  in  Simulink  and  QuaRC, 
an  interactive  3D  environment  with  haptic  feedback  can  be  created,  allows 
for  simulation  or  training  applications. 
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C.  GROUND  CONTROL  STATION 

The  ground  control  station  is  comprised  of  a  PC  which  is  equipped  with  the 
required  research  software,  including  MATLAB  /  Simulink  with  real-time  control 
software  QuaRC  and  the  OptiTrack  Tracking  Tools  program  the  OptiTrack  camera 
system.  In  addition,  wireless  communications  are  achieved  through  a  wireless  network 
adapter,  inserted  to  the  ground  control  station  computer  while  a  USB  2.0  port  is  used  for 
connecting  the  OptiTrack  indoor  localization  system.  The  distributed  control  structure 
allows  the  real-time  code  to  be  generated  from  the  host  controller,  wirelessly  be  sent 
through  QuaRC  to  any  vehicle  where  it  would  be  compiled  and  executed  on-board  it.  The 
ground  control  station  can  operate  with  several  vehicle  controllers  at  the  same  time. 

The  researcher  is  able  to  perform  several  tasks  through  the  ground  control  station 
computer: 

•  Developing  controllers  for  the  navigation  of  the  unmanned  vehicles, 

•  using  the  OptiTrack  Tracking  Tools  program  to  calibrate  the  localization 
system,  in  order  to  provide  wireless  localization  data, 

•  view  Simulink  Scopes  and  several  display  tools  to  monitor  the  vehicles 
and  their  status  during  mission  carrying  out, 

•  tune  and  update  the  various  controller  parameters  to  improve  perfonnance 
during  runtime 

•  log  and  monitor  the  mission  runtime  data. 

•  conduct  vision  and  image  processing 


D.  HiQ  -GUMSTIX  EMBEDDED  COMPUTER 

The  HiQ  embedded  avionics  data  acquisition  card,  shown  in  Figure  10,  developed 
by  Quanser  company  was  integrated  with  the  small  fonn-factor,  lightweight  Gumstix 
Verdex  embedded  computer  [53]  with  wireless  communications  capabilities  that  runs  a 
Linux-based  operating  system  in  order  to  achieve  measuring  the  behavior  of  the 
unmanned  systems  being  controlled  and  be  able  to  run  generated  simulink  models 
designed  with  QuaRC  software. 
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The  Gumstix  computer  has  been  configured  as  a  QuaRC  target  system  to  create 
the  designed  controllers  in  the  ground  station  computer  and  download  and  execute  them 
on  the  QuaRC  target  computer  without  requiring  embedded  programming.  HiQ  card 
provides  high-resolution  inertial  measurement  sensors  and  motor  outputs,  and  is 
responsible  for  the  reading  and  writing  to  vehicle  hardware.  QuaRC’ s  Hardware-In-the- 
Loop  (HIL)  tools  give  directly  access  to  the  Input  /  Output  (I/O)  for  any  supported  DAC, 
including  the  HiQ. 


◄ - ► 

10.54cm 


Figure  10.  HiQ  embedded  avionics  data  acquisition  card.  After  [55]. 

The  Input  /  Output  of  the  HiQ  card,  as  it  stated  in  [55]  consists  of: 

•  10  PWM  outputs  (servo  motor  outputs) 

•  3-axis  gyroscope,  range  configurable  for  ±75°/s,  ±150Ys,  or  ±300%, 
resolution 

•  0.01832°/s/LSB  at  a  range  setting  of  ±75°/s 

•  3-axis  accelerometer,  resolution  2.522  mg/LSB 

•  10  analog  inputs,  12-bit,  +3.3V 

•  3-axis  magnetometer,  0.76923  mGa/LSB 

•  4  Maxbotix  sonar  inputs,  1  inch  resolution 

•  Serial  GPS  input 

•  8  channel  RF  receiver  input 

•  USB  input  for  on-board  camera  (up  to  9fps) 

•  2  pressure  sensors,  absolute  and  relative  pressure 

•  Input  power  10-20V 
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E. 


LOCALIZATION  SYSTEM 


Indoor  laboratories  have  to  utilize  alternative  localization  systems  in  order  to 
track  vehicle  positions  in  the  area  of  the  operations,  since  no  access  to  GPS  exists.  The 
OptiTrack  Tracking  Tools  system,  used  in  this  laboratory,  is  created  by  Natural  Point 
company  and  designed  for  indoor  applications.  It  is  a  camera-based  localization  and 
tracking  system  integrated  with  QuaRC  and  consists  of  a  series  of  at  least  six  motion 
tracking  OptiTrack  cameras  connected  to  the  ground  station  computer.  The  quantity  of 
the  cameras  depends  on  the  complexity  of  the  environment;  with  the  increase  of  the 
quantity  to  reduce  the  tracking  loss  possibility.  The  QuaRC  OptiTrack  block  set  allows 
operators  to  track  multiple  reflective  markers  simultaneously  in  3-D  space  and/or  fixed 
patterns  of  markers  (rigid  bodies)  in  6-DOF  position  and  orientation.  The  features  of  the 
OptiTrack  vision  system  as  stated  in  [55]  are  listed  below: 

•  Up  to  16  cameras  (Figure  12)  can  be  connected  and  configured  for  single 
or  multiple  capture  volumes,  for  the  experiment  ten  cameras  were  chosen. 

•  Capture  volumes  up  to  400  square  feet 

•  Single  point  tracking  for  up  to  80  markers,  or  10  rigid-body  objects 

•  Typical  calibration  time  is  under  5  minutes 

•  Position  accuracy  on  the  order  of  mm  under  typical  conditions 

•  USB  2.0  connectivity  to  ground  station  PC 

•  Up  to  100  fps  tracking 


Figure  11.  Natural  Point  OptiTrack  camera,  adapted  from  [55]. 
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More  specifically,  the  ten  cameras  were  chosen  to  be  in  the  position  shown  in 
Figure  9  in  order  to  eliminate  the  blind  areas  for  every  possible  scenario  that  it  will  be 
taken  place.  The  connection  should  have  implemented  through  the  right  configuration 
since  the  data  from  the  cameras  can  be  transferred  successfully  if  the  cables  are  up  to  5 
meters,  that’s  why  three  different  hubs  had  to  be  used  in  specific  location  to  fulfill  that 
requirement.  Each  hub  had  to  be  connected  to  computer  with  uplink  USB  cables.  Since 
those  hubs  are  quite  far  away  from  each  other,  extension  cables  were  used.  Each  hub  can 
also  connect  with  up  to  6  cameras  with  high  speed  USB  cables,  although  we  used  three  or 
four  only  ports.  The  camera  and  the  documented  required  configuration  for  the  setup  is 
shown  in  Figure  12. 


Figure  12.  Optihubs  Setup  with  Optitrack  Cameras.  After  [55]. 
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After  installing  and  power  the  whole  system  calibration  of  the  cameras  is  needed. 
This  procedure  will  be  executed  through  the  Optitrack  Tracking  Tools  program,  which  is 
designed  by  the  Optitrack  Company  and  allows  the  operator  to  watch  and  adjust  all  the 
cameras’  position  into  the  laboratory  and  the  view  coming  from  each  of  them.  This 
program  is  compatible  with  Quarc  software,  needed  for  controlling  the  vehicles.  The 
calibration  procedure  is  actually  very  easy  and  basically  two  tools  are  needed,  a  trident 
and  a  T-shape  (gamma-shape)  zero  point  instrument. 

Firstly,  the  operator  starts  the  “tracking  tools”  program.  He  has  then  to  conduct 
two  visual  checks;  checking  that  each  camera  is  shown  inside  the  program  so  that  the 
connection  is  successful  and  that  there  is  no  reflection  coming  from  the  laboratory  space. 
Then,  he  starts  moving  the  trident  in  a  figurate  pattern  (wanding  process),  whose  edges 
have  reflectors  on  the  top,  within  the  whole  space  trying  to  cover  every  possible  view 
angle  of  each  of  the  cameras  He  continues  doing  so  until  the  tracking  tools  program 
indicates  that  all  the  cameras  have  calibrated  successfully.  Next,  the  zero  point  has  to  be 
adjusted  with  the  T-shape  instrument,  where  it  shows  the  two  axes,  x  and  y.  It  is 
advisable  that  the  vehicle  has  to  be  placed  at  this  point  in  the  beginning  before  the 
movement  starts.  Finally,  since  the  calibration  has  completed,  the  calibration  file  has  to 
be  saved  in  order  to  be  used  in  each  Simulink  vehicle  model. 

In  order  to  localize  the  vehicles,  this  exact  file  has  to  be  used  inside  the  Quarc’s 
software  and  especially  the  “Optitrack  Point  Cloud”  Simulink  block,  a  block  that  gives 
the  position  of  the  vehicle  markers  tracked  by  the  Optitrack  camera  system.  After  that 
every  model  is  ready  to  be  used  for  any  operation.  Through  that  block  you  can  always 
check  how  many  markers  are  shown  and  if  they  are  tracked  successfully  and  in  the  event 
of  an  additional  marker,  thus  unwanted  reflection,  to  stop  the  operation  and  conduct 
another  calibration  after  eliminating  the  issue.  The  cameras  inside  the  laboratory  are 
mounted  accordingly  and  once  adjusted,  remain  stable  so  that  there  is  no  need  for 
continuous  calibration. 
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F.  QUANSER  QUADROTOR  Qball-X4 

1.  Introduction 

The  Qball-X4  quadrotor  helicopter  designed  and  constructed  by  the  Quanser 
Company,  is  a  rotary  wing  vehicle  platfonn  enclosed  within  a  protective  carbon  fiber 
cage  that  is  propelled  by  four  motors  fitted  with  10-inch  propellers.  The  particular  design 
ensures  safe  operation  in  an  indoor  laboratory  environment  usually  surrounded  with 
various  close  range  obstacles  or  other  vehicles  and  it  can  be  used  in  a  wide  variety  of 
UAV  research  applications. 

Qball-X4  utilizes  Quanser's  onboard  avionics  data  acquisition  card  (DAQ),  called 
HiQ  that  cooperates  with  an  embedded  computer  by  Gumstix  in  order  to  read  the  on¬ 
board  sensors  of  the  vehicle  and  drive  the  motors.  The  QBall-X4  is  an  open-architecture 
UAV,  allowing  operators  to  rapidly  create  and  deploy  unmanned  vehicle  controllers 
ranging  from  low-level  flight  dynamics  stabilization  to  advanced  multi-agent  guidance, 
navigation  and  control  algorithms.  Other  specifications  of  the  QBall-X4  include: 

•  Diameter  0.7m  -  height  0.6m 

•  2  LiPo  batteries,  2500mAh,  3-cell 

•  15  minute  flight  time  per  charge 

•  (740Kv)  motors  fitted  with  10  inch  propellers 

•  Protective  cage  made  from  carbon  fiber  enclosing  the  quadrotor 

•  USB  camera  up  to  9  frames  per  sec  color  images 

•  Wireless  communications  capability 

QUARC,  Quanser’s  real-time  control  software,  allows  the  operator  to  rapidly 
develop  and  test  controllers  on  the  host  computer  through  a  MATLAB  Simulink 
interface,  whereas  these  models  can  be  generated  and  executed  on  the  Gumstix  embedded 
computer  automatically  in  real  time.  At  the  same  time  operator  can  observe  sensor 
measurements  and  tune  the  various  parameters  from  the  host  ground  station  computer 
(PC  or  laptop). 
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The  communication  hierarchy  for  the  operation  of  Qball-X4  is  shown  in  Figure 
13. 


Target 


Wifi 

Send  code  to  target  (Gumstix) 
Send/Receive  scope  data, 
update  runtime  parameters 


Runs  model  (controller) 


Host 


HiQ  and  Gumstix 


Generate  code 


Figure  13.  Communication  Hierarchy.  After  [51]. 


2.  X4  Diagram 

The  basic  diagram  of  the  Qball-X4  quadrotor  with  the  axes  and  angles  is  shown  in 
Figure  14,  with  the  X  axis  aligned  with  the  front  of  the  vehicle.  It  is  very  common  the  tail 
or  back  of  the  vehicle,  marked  with  colored  tape,  to  point  toward  the  operator  and  the 
positive  X  axis  away  from  him  when  the  vehicle  operates. 


Figure  14.  Qball-X4  axes  and  sign  convention.  After  [51]. 
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3. 


Main  Components 


The  main  components  of  Qball-X4  are  described  below  [51]: 
a.  Qball-X4  Protective  Cage  and  Frame. 

Qball-X4  quadrotor  rests  inside  a  protective  frame  (Figure  2.4)  which  is  a 
crossbeam  structure  to  which  the  Qball-X4  components  are  mounted.  The  Qball-X4's 
protective  cage  is  a  carbon  fiber  structure  designed  to  protect  the  frame,  motors, 
propellers,  embedded  control  module  (HiQ  and  Gumstix)  and  speed  controllers  during 
minor  collisions,  since  it  is  not  intended  to  withstand  large  impacts  or  drops  from  heights 
greater  than  2  meters.  That’s  why  in  order  to  move  or  lift  it,  someone  has  to  carry  it  from 
the  ends  of  the  frame  from  both  sides  as  it  is  shown  in  figure  15. 


Figure  15.  Qball-X4  cage  and  frame.  After  [51] 

b.  QUARC Data  Acquisition  Card  (HiQ  DAQ)  -  Gumstix  Computer. 

The  HiQ  DAQ  shown  in  Figure  2.6  is  a  high-resolution  inertial 
measurement  unit  (IMU)  and  avionics  input/output  (I/O)  data  acquisition  card  that 
cooperating  with  the  Gumstix  embedded  computer  controls  the  vehicle  by  having  inputs 
from  the  sensors  on  board  and  sending  the  motor  commands.  Each  motor  speed  controller 
is  connected  in  a  specific  order  to  one  of  the  ten  PWM  servo  output  channels  that  are 
available  on  the  HiQ,  in  order  for  the  associated  controllers  to  be  operated.  An  optional 
daughterboard  that  contains  additional  I/O  such  as  receiver  or  sonar  inputs  or  a  TTL 
serial  input  used  for  a  GPS  receiver.  The  standard  servo  output  channels  associated  with 
every  motor  and  the  most  common  channel  associated  with  sonar  in  the  daughterboard 
are  shown  in  Table  3. 
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Figure  7:  HiQ  DAQ-HiQ  daughterboard  with  optional  receiver  inputs.  After  [51]. 


Motor 

Servo  Output  Channel 

Back 

0 

Front 

1 

Left 

2 

Right 

3 

Daughterboard 

Sonar 

0 

Table  3.  Servo  Output  Channel 


Figure  16.  HiQ  cover.  After  [51]. 
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c.  Qball-X4  Power  -  LiPo  Batteries  -  Switches  -  Connectors 

The  power  to  the  Quadrotor  (HiQ  and  motors)  is  provided  by  two  3 -cell 
2500mAh  LiPo  batteries  (#14  in  Figure  17)  which  should  be  secured  tightly  in  a  vertical 
position  with  velcro  straps  on  the  bottom  side  of  the  frame.  After  connecting  the  batteries 
with  the  battery  connectors,  the  power  is  turned  on  by  using  two  switches  (one  for  each 
battery)  (#12  in  Figure  17).  Because  of  the  fact  that  these  batteries  will  be  damaged  and 
turned  to  no  use  if  they  are  discharged  below  10  V,  they  should  be  fully  charged  when 
they  reach  10.6  V  or  less. 


Figure  17.  Battery  switch  and  connector.  After  [51]. 

d.  Motors,  Propellers  (10x4. 7)  and  Speed  controllers  (ESCs) 

The  motors  used  in  the  quadrotor  are  four  E-Flite  Park  400  (740  Kv) 
motors  (Figure  18)  fitted  with  paired  counter-rotating  APC  10x4.7  propellers  [4]  (#16  in 
Figure  18),  mounted  and  connected  with  the  four  electronic  speed  controllers  (ESCs)  [5], 
along  the  X  and  Y  axes  of  the  frame. 

The  motors  and  propellers  are  configured  so  that  the  front  and  back  motors 
spin  clockwise  and  the  left  and  right  motors  spin  counterclockwise.  The  ESCs  receive 
commands  from  the  HiQ  in  the  form  of  PWM  outputs  from  1ms  (minimum  throttle)  to 
2ms  (maximum  throttle)  or  can  be  configured  to  set  the  throttle  range,  but  always  with 
initial  PWM  outputs  to  the  minimum  throttle  value  of  0.05. 
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Figure  18.  Motor  and  propeller-ESCs  and  batteries.  After  [51]. 


Figure  19.  Optional  sonar  and  sonar  mount.  After  [51]. 


ID# 

Description 

ID# 

Description 

1 

Qball-X4  protective  cage 

10 

GPS  serial  input 

2 

Qball-X4  frame 

II 

Battery  switch 

3 

HiQ  DAQ  with  Gunistix 

12 

Battery  connector 

4 

HiQ  inertial  measurement  unit 

13 

Speed  controllers  (ESCs) 

5 

HiQ  servo  PWM  outputs 

14 

LiPo  batteries 

6 

HiQ  cover 

15 

Optional  sonar 

7 

HiQ  daughterboard  with  optional 
receiver 

16 

Propeller  (10x4.7) 

8 

Receiver  inputs 

17 

Motor 

9 

Sonar  inputs 

Table  4.  Description  of  Qball’s  main  components.  After  [51]. 
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4.  System  Set  up 


a.  Wireless  Connection 

The  quadrotor  communicates  with  the  host  computer  and  the  ground  robot 

by  utilizing  an  ad-hoc  peer-to-peer  wireless  TCP/IP  connection  through  a  USB  wireless 

adapter.  The  network  established  is  called  GSAH  and  is  an  unsecured  one,  with  the 

following  settings  /  properties: 

IP  address:  182.168.1.xxx  (any  unused  number) 

Subnet  mask:  255.255.255.0 

Default  gateway:  182.1 68. l.xxx  (same  as  IP) 


Figure  20.  Wireless  USB  adapter  settings 

b.  Qball-X4  Vehicle  Setup 

After  checking  that  the  motors,  propellers  are  firmly  secured  to  the  frame 
regularly  (after  every  2  hours  of  flight),  the  batteries  must  be  installed  and  connected  to 
the  battery  connectors  in  the  way  described  above  and  is  shown  in  Figure  21. 
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Figure  2 1 .  Batteries  secured  with  velcro  straps.  After  [51]. 


To  power  on  the  Qball-X4,  the  two  power  switches  connected  to  the 
battery  cables  (#11  in  Figure  8)  must  be  turned  on,  initiating  the  Gumstix  wireless 
module  one  minute  after.  Then  if  connected  to  the  GSAH  ad-hoc  network  on  the  host  PC, 
the  quadrotor  is  ready  to  be  navigated. 

c.  Quanser  Real-Time  Control  (QuaRC)  Software  Configuration 

After  installing  QuaRQ  software,  a  new  item  will  be  added  in  Simulink 
menu.  Some  configurations  must  be  done  to  be  able  to  run  any  QuaRC  model  on  the 
target  vehicle. 

First  of  all,  the  target's  Gumstix  IP  address  must  be  specified.  So,  the 
operator  has  to  setup  the  default  target  IP  address  for  all  targets,  inside  theQuaRC  menu 
through  the  Preferences  option  (e.g.,  “tcpip://182. 168. 1.200:17001”). 

Moreover,  “External”  simulation  mode  must  be  selected  instead  of 
“Normal”  to  run  the  model  on  the  target  machine  (Gumstix),  otherwise  only  a  simulation 
will  be  run  on  the  host  machine. 

Finally,  the  building  of  the  model  (QuaRC/Build)  only  remains  in  order  to 
begin  the  code  generation  and  compiling  steps  on  the  target  vehicle.  QUARC  console 
will  show  when  the  compilation  will  be  finished  (usually  this  process  takes  some 
minutes). 
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d.  Qball-X4  Sensors  Associated  with  QuaRC  Blocks 

QuaRC's  open-architecture  hardware  and  extensive  Simulink  block  set 
provides  users  with  powerful  controls  development  tools.  Several  of  these  blocks  of  the 
QuaRQ  software  have  to  be  used  in  order  to  read  the  sensors  attached  in  the  quadrotor 
and  provided  the  data  in  Simulink  models.  The  most  important  are  the  Hardware-In-the- 
Loop  (HIL)  block  for  communication  with  HiQ  board.  The  HIL  Initialize  block  for 
initialization  the  HiQ  and  setup  the  I/O  parameters  and  the  HIL  Read  Write  block  for 
reading  and  writing  from  the  HiQ  to  the  model. 

The  quadrotor’ s  commands  for  the  four  motors  are  associated  with  the 
PWM  outputs  0  to  3.  The  range  of  the  PWM  output  values  is  between  0.05  (zero  throttle) 
which  corresponds  to  a  1ms  pulse  (5%  of  a  20ms  duty  cycle)  and  0.10  (full  throttle), 
equivalent  to  2ms  pulse  (10%  of  a  20ms  duty  cycle). 

To  control  the  flight  of  the  quadrotor  several  measurements  are  required 
from  the  IMU.  The  input  from  the  magnetometer,  functioning  as  a  digital  compass  is  used 
to  measure  the  heading  (corresponding  yaw  angle)  while  the  3 -axis  gyroscope  and 
accelerometer  inputs  are  used  to  measure  the  quadrotor  dynamics  and  orientation  (roll, 
pitch  and  yaw  angles). 

As  already  noted,  the  LiPo  batteries  should  be  charged  at  10.6V,  otherwise 
the  batteries  will  be  destroyed.  The  block  “Show  Message  on  Host”  allows  the  operator 
to  check  the  battery  capacity  by  displaying  a  low  battery  warning  message  on  the  host 
PC.  The  operating  capacity  input  measures  the  battery  value  as  a  percentage  (0-1%)  of 
the  quadrotor’ s  input  voltage  range  from  the  minimum  value  of  10V  to  the  maximum  one 
of  20V  (10.6V  is  equivalent  to  0.06  or  6%). 
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G.  QUANSER  QBOT  GROUND  VEHICLE 


1.  Introduction 


The  Quanser  Qbot  (Figure  22)  is  an  autonomous  ground  robot  vehicle  consists  of 
the  iRobot  Create  platfonn  widely  used  in  robot  applications  with  four  infrared  and  three 
sonar  sensors  and  a  Logitech  Quickcam  Pro  9000  USB  camera  mounted  on  the  vehicle, 
and  the  embedded  Quanser  Controller  Module  (QCM)  utilizing  a  Gumstix  computer  so  as 
to  run  QuaRC,  Quanser's  real-time  control  software  and  the  Qbot  data  acquisition  card 
(DAC).  So,  in  other  words,  Quanser  has  taken  the  iRobot  Create  platform  and  augmented 
its  sensor  capabilities  by  adding  [54]: 

•  8  PWM  outputs  for  servo  motors 

•  7  reconfigurable  digital  I/O,  plus  1  digital  output  LED 

•  7  analog  inputs,  12-bit,  +5V  inputs,  resolution  6.2  mV 

•  5  infra-red  (IR)  sensors  up  to  150cm 

•  3  sonar  sensors  15cm  to  6.45m,  1-inch  resolution 

•  3-axis  magnetometer,  resolution  of  0.77  milli-Gauss 

•  USB  camera  up  to  9fps  color  images 

•  Wireless  communications 


Figure  22.  The  Quanser  Qbot  -  front  (Left)  and  top  (Right)  view.  After  [54]. 


The  QCM  interface  is  a  MATLAB  Simulink  with  QuaRC  as  in  the  quadrotor.  The 
Qbot  is  accessible  through  three  different  block  sets:  the  Roomba  block  set  to  drive  the 
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vehicle,  the  HIL  block  set  to  read  from  sensors  and/or  write  to  servo  outputs,  and  finally 
the  OpenCV  block  set  to  access  the  camera.  The  controllers  are  developed  in  Simulink 
with  QuaRC  on  the  host  computer,  and  these  models  are  downloaded  and  compiled  into 
executables  on  the  target  [54]  seamlessly.  A  diagram  of  this  configuration  is  shown  in 
Figure  23  and  the  characteristics  of  the  Qbot  vehicle  is  shown  in  Table  5. 


Figure  23.  Communication  Hierarchy.  After  [54], 


Qbot  Specifications 

Symbol 

Description 

Value 

Unit 

D 

Diameter 

0.34 

m 

H 

Height 

(With  Camera  Attachment) 

0.19 

m 

Vmax 

Maximum  speed 

0.5 

m/sec 

M 

Total  mass 

2.92 

Kg 

Table  5.  Qbot  Model  Parameters  and  System  Specifications.  From  [54] 
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2. 


Main  Components  Description 


The  main  components  of  the  Qbot  are  described  in  the  next  section  [54]. 
a.  The  iRobot  Create® 

The  Qbot  uses  an  iRobot  Create®  frame  (Figure  24).  The  Qbot  body 
frame  axes  are  the  Quanser  standard,  where  the  x-axis  is  in  the  forward  direction,  the  y- 
axis  is  to  the  left,  and  the  z-axis  is  up.  The  iRobot  Create®  has  a  bumper  sensor  and  an 
omni-directional  infrared  receiver.  The  QCM  is  capable  to  read  these  sensors  and 
receiving  their  data.  Two  differential  drive  wheels  drive  the  vehicle  of  diameter  of  34  cm 
and  height  (without  camera  attachment)  of  7  cm. 


6-32  Mounting 
Cavities 


DB-25 


Figure  24.  Anatomy  of  Qbot,  showing  various  components  and  body  axes. 

After  [54]. 

The  Qbot  is  turned  on  by  pressing  the  power  button.  Built-in  demos  for  the 
vehicle  are  also  available  through  the  Play  and  Advance  buttons  (Figure25). 

Play 

Power  Button  Button  Advance  Button 


Power  LED 


Play  LED  Advance  LED 


Figure  25.  Buttons  on  the  Qbot  Frame.  After  [54]. 
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b.  QbotDAC 

The  Qbot  DAC  is  a  data  acquisition  board,  located  underneath  the  black 
cover  of  the  Qbot,  existed  for  receiving  analog  inputs  and  sonar  inputs  or  any  other  input 
from  different  optional  sensors.  It  is  also  capable  of  writing  PWM  outputs  for  possible 
servo  actuators. 


c.  Gumstix  Computer 

The  Gumstix  is  a  small-scale,  fully  functional,  open  source  computer  at 
where  the  MATLAB/  Simulink  models  are  directly  downloaded,  compiled,  and  executed 
through  the  QuaRC  software.  The  Gumstix  motherboard  is  connected  directly  to  the  Qbot 
DAC.  Wifi  attachment  board  to  allow  wireless  connection  between  the  target  Gumstix 
and  the  host  computer  and/or  other  vehicles  is  also  available. 

d.  The  Printed  Circuit  Board  (PCB) 

The  wiring  and  circuitry  for  the  Qbot  in  the  printed  circuit  board  (PCB) 
that  is  located  on  the  black  cover  of  the  Qbot  over  the  Gumstix  computer  and  the  DAC. 
The  sensors  and  the  webcam  are  also  mounted  on  the  PCB.  Figure  26  shows  the 
accessible  pins  for  the  user.  In  particular,  the  DIO,  PWM  output,  and  analog  input  pins 
have  been  labeled  for  clarity. 


Legend  1  I  .Analog  Input  l  l  PWM  Output  l  l  Digital  Input  Output 
□  SWnSW  Jumper  1  1  EXT'INT  Jumper 


□  “D* 

Figure  26.  Qbot  PCB  showing  available  pins  for  the  PWM  output, 

Digital  Input/Output  and  Analog  Input  pins,  and  jumpers  for  INT/EXT 

power.  After  [54]. 
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e. 


Digital  Input/Output  Pins  (DIO  #) 


The  DIO  channels  (0  to  6)  are  set  as  inputs  by  default.  With  the  HIL 
Initialize  block,  the  operator  can  configure  the  DIO  channels  either  as  inputs  or  outputs 
(not  both).  There  is  also  a  fixed  output  indicated  by  a  LED  labeled  DI07  associated  with 
a  final  digital  channel  (7). 

f  SW/nSW  and  INT/EXT  Jumpers 

The  operator  can  switch  between  internal  power  from  the  iRobot  Create 
battery  (INT)  and  an  external  battery  power  supply  (EXT)  with  the  power  source  jumper 
INT/EXT  jumper.  To  power  the  Qbot,  the  jumper  should  be  placed  in  the  INT  position 
and  then  the  SW/nSW  jumper  will  indicate  if  the  iRobot  Create  must  be  switched  on 
(SW)  for  the  Qbot  to  receive  power,  or  whether  the  Qbot  should  always  draw  power  even 
when  the  iRobot  Create  is  off  (nSW). 

g.  USB  Camera 

The  camera  is  mounted  on  top  of  the  Qbot  (Figure  27).  The  QuaRC  block 
set  that  uses  the  Open  Source  Computer  Vision  library  giving  the  opportunity  to  the 
operator  to  capture  and  display  images  in  real  time,  process  them,  and  even  save  them. 
The  image  Logitech  Quickcam  Pro  9000  USB  resolution  though  is  low. 


Figure  27.  Logitech  Quickcam  Pro  9000  USB  Camera.  After  [54]. 
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h.  Battery 

The  Qbot  is  powered  by  the  Advanced  Power  System  (APS)  Battery 
(Figure  28-Left)  provided  by  iRobot  and  placed  in  specific  location  under  the  Qbot 
(Figure  28-Right).  If  the  battery  is  fully  charged,  is  able  to  last  continuously  for  about  2 
hours.  The  power  level  of  the  battery  is  designated  by  the  power  light  (green  for  fully 
charged  /  red  for  discharged  batteries)  on  the  Qbot  platform.  The  battery  takes  about  3 
hours  to  fully  charge. 


Figure  28.  The  Qbot  the  Advanced  Power  System  Battery  (Left  picture)  - 
Battery  location  highlighted  in  the  Bottom  view  of  the  Qbot. 

After  [54]. 

i.  Infrared  Sensors  -  Sonar  Sensors 

Qbot  has  five  SHARP  2Y0A02  low  cost  infrared  range  sensors  (20-150 
cm)  (Figure  29-Left)  and  three  MaxSonar-EZO,  very  short  to  long  range  detection  and 
ranging,  sonar  sensors  (Figure  29-Right),  that  are  connected  to  the  analog  input  channels 
of  the  Qbot  DAC.  Readings  of  those  sensors  can  be  taken  through  the  HIL  Read  Write 
block  of  the  QUARC  software.  The  sonar  provides  detection  range  between  0  and  254- 
inches  (6.45  meters)  while  the  information  provided  covers  range  from  6-inches  out  to 
254-inches  with  resolution  of  1  inch. 

Figure  29.  SHARP  GP2D12  IR  Sensor  (Left)  -  LV-MaxSonar-EZO  Sonar  Range 

Finder  (Right).  After  [54]. 
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3.  System  Setup 

a.  Setting  up  the  Qbot 

To  set  up  the  Qbot  the  operator  has  to  follow  two  steps: 

(1)  Insert  the  battery  under  the  Qbot  to  the  designated  area. 

(2)  Press  the  power  button  to  turn  on  both  the  robot  and  Qbot 

DAC/Gumstix. 


b.  Establishing  Wireless  Connection 

The  Qbot  communicates  with  the  host  computer  and  the  quadoror  by 
utilizing  an  ad-hoc  peer-to-peer  wireless  TCP/IP  connection  through  a  USB  wireless 
adapter.  The  network  established  is  the  same  GSAH  one,  with  the  settings  /  properties 
described  before  for  the  quadrotor. 

c.  Quanser  Real-Time  Control  (QuaRC)  Configuration 

Having  installed  QUARQ  software,  the  same  configurations  with  the 
quadrotor  have  to  be  done  for  the  Qbot  as  well,  so  as  to  be  able  to  run  any  QUARC 
model  on  the  target  vehicle. 

First  of  all,  the  target's  Gumstix  IP  address  must  be  specified.  So,  the 
operator  has  to  setup  the  default  target  IP  address  for  all  targets,  inside  the  QUARC  menu 
through  the  Preferences  option  (e.g.,  “tcpip://182. 168. 1.200:17001”). 

Moreover,  “External”  simulation  mode  must  be  selected  instead  of 
“Normal”,  so  as  to  run  the  model  on  the  target  machine  (Gumstix),  otherwise  only  a 
simulation  will  be  run  on  the  host  machine. 

Next,  the  building  of  the  model  (QUARC/Build)  only  remains  in  order  to 
begin  the  code  generation  and  compiling  steps  on  the  target  vehicle.  QUARC  console 
will  show  when  the  compilation  will  be  finished  (usually  this  process  takes  some 
minutes). 
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d.  Qbot  Sensors  Associated  with  QuaRC  Blocks 

Several  blocks  of  the  QUARQ  software  have  to  be  used  in  order  to  read 
the  sensors  attached  in  the  Qbot  and  provided  the  data  in  Simulink  models.  The  most 
important  are  described  below  [54]: 

(1)  The  Roomba  Initialize  block  located  in  the  Simulink 
Library  Browser,  under  QuaRC  Targets  /  Devices  /  Third-Party  /  iRobot  /  Roomba  / 
Interfacing,  is  required  for  the  Gumstix  computer  to  communicate  with  the  Qbot.  The 
operator  must  change  the  local  host  from  the  default  2  to  1 . 

(2)  The  HIL  Initialize  block  located  in  the  Simulink  Library 
Browser,  under  QuaRC  Targets  /  Data  Acquisition  /  Generic  /  Configuration,  is  required 
to  communicate  with  the  Qbot  DAC.  When  model  runs,  the  board  type  "qbot"  must  be 
selected  in  order  to  target  the  Qbot. 

(3)  The  HIL  Read  Write  block  located  in  the  Simulink  Library 
Browser,  under  QuaRC  Targets  /  Data  Acquisition  /  Generic  /  Immediate  I/O  is  required 
to  read  infrared  sensor  (analog)  and  sonar  measurements  from  Qbot. 

(4)  The  blocks  located  in  the  Simulink  Library  Browser,  under 
QuaRC  Targets  Beta  /  Image  Processing  /  Open  Source  Computer  Vision  are  required  to 
use  the  on-board  Logitech  camera  for  image  processing. 
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H.  TOWARD  UNIFIED  CONTROL  ARCHITECTURE 


From  the  standpoint  of  controls  QBall  and  QBot  are  shown  in  Figures  30  and  31, 
respectively.  On  the  left  the  control  inputs  that  affect  vehicles  dynamics  and  kinematics 
are  introduced.  On  the  right  the  available  output  signals  are  shown.  The  position  feedback 
is  provided  by  the  motion  capture  cameras.  While  both  systems  are  capable  to  operate 
autonomously,  the  communication  with  the  control  station  is  provided  via  a  wireless 
connection.  Multiple  systems  can  talk  to  each  other  directly  as  well. 


Quadrotor  QBall-X4  Architecture 


X,  Y  Position 

Through  Motion  Canture  Cameras 


Vright 


Vleft 


- ► 

Visual  Camera  Output 

- ► 

IR  Cameras  Output 

- ► 

Sonars  Output 

- * 

Bumper  Output 

- ► 

STATES:  X,  dotX,  Y  dotY,  6,  doth 

Wireless  Connection  to 

the  control  station 

Figure  30.  Ground  Robot  QBot  Architecture 


Table  6  shows  sensors  available  on  both  vehicles  side  by  side. 


42 


Vehicles 

QBall  -  X4 

QBot 

Sensors 

IR  Camera 

- 

+ 

Visual  Camera 

- 

+ 

Sonar 

+ 

+ 

Bumper 

- 

+ 

Accelerometer 

+ 

- 

Magnetometer 

+ 

- 

Table  6.  Comparison  of  the  two  vehicles  sensors 


QBall  can  also  be  equipped  with  the  IR/EO  camera.  In  tenns  of  performance, 

•  QBall  has  a  large  velocity  compare  o  that  of  QBot  which  opportunity  leads 
to  the  fast  accomplishment  of  mission. 

•  The  higher  height  of  QBall  operation  results  in  larger  view  of  the  field  of 
operation  and  better  operation  coverage. 

•  QBot  has  a  larger  payload  /  sensor  capability  and  therefore  better  obstacle 
avoidance  and  identification  of  the  target  capability. 

Hence  these  two  vehicles  complement  each  other,  and  that’s  why  their 
cooperation  could  improve  the  chances  of  mission  success. 
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III.  MODELLING  OL  QUADROTOR  AND  GROUND  ROBOT 


In  this  chapter,  the  dynamic  and  kinematics  modeling  of  the  quadrotor  and  the 
ground  robot  will  be  presented  to  the  extent  of  deriving  the  final  equations  describing  the 
vehicles  and  that  they  will  be  used  for  the  design  of  the  controllers.  The  general  dynamic 
model  of  the  quadrotor  has  been  presented  in  many  papers  and  can  be  derived  through 
two  different  approaches,  the  Euler  -  Newton  and  the  Euler  -  Lagrange  formalisms,  see 
[1].  The  modeling  of  a  ground  robot  is  modeled  with  the  forward  and  the  inverse 
kinematics;  see  [56]  and  [57]. 

A.  MODELLING  OF  QUADROTOR  QBALL-X4 

In  order  to  start  modeling  the  quadrotor,  the  coordinate  frames  that  were  used, 
have  to  be  presented,  described  and  defined. 

1.  Coordinate  Frames 

a.  Earth-Fixed  Inertial  Frame  U 

The  inertial  frame  is  established  by  the  direction  of  the  Earth’s  rotation. 
The  coordinate  frame  to  be  used  is  the  North-East-Up,  with  the  origin  0(J  generally  at  an 

arbitrary  ground  point,  here  chosen  to  be  at  the  quadrotor  take-off  point.  As  shown  in 
figure  32,  the  unit  vector  ‘x’  points  toward  North,  the  unit  vector  V  toward  East  and  ‘z’ 
toward  the  opposite  direction  of  the  center  of  the  Earth. 


Figure  3 1 .  Earth  Fixed  Inertial  Frame  U 
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b.  Body-  Fixed  Frame  B 

The  body  frame  is  rigidly  attached  to  and  defined  within  the  vehicle  and 
exists  to  specify  quadrotor’s  orientation.  The  definition  for  this  frame  has  the  x-axis  along 
the  two  opposing  propellers  axis,  the  y  axis  pointed  along  the  other  two  opposing 
propellers  and  the  z-axis  upward,  forming  the  right  hand  set.  This  frame  is  shown  below 
in  figure  33  and  Roll,  Pitch  and  Yaw  are  defined  as  the  angles  of  rotation  tp,  0  and  v| / 
about  x,  y  and  z  axis,  respectively.  The  origin  of  the  body  frame  is  at  the  center  of  the 
mass  of  the  quadrotor. 


Origin  0B 
(CM) 

Figure  32.  Body-fixed  Frame  B 

2.  Modeling  Assumptions 

At  this  point  several  reasonable  assumptions  concerning  the  modeling  of  the 
quadrotor  must  also  be  made: 

•  The  Earth  is  flat,  non-rotating,  and  an  approximate  inertial  reference 
frame. 

•  The  acceleration  of  gravity  is  constant  and  perpendicular  to  the  surface  of 
the  Earth. 

•  The  design  is  symmetrical  with  respect  to  the  axis. 

•  The  quadrotor  body  as  well  as  the  propellers  will  be  treated  as  rigid  body, 
so  that  the  Newton-  Euler  Formulas  can  be  used. 

•  Since,  it  is  an  indoor  experiment,  and  the  speed  is  considered  to  be  low, 
the  air  friction  will  be  ignored,  gyroscopic  effects  and  the  aerodynamic 
torques  can  be  cancelled. 

•  No  swash  plate  exists  for  each  rotor. 
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3. 


Vehicle  State  Variables 


The  quadrotor  QBALL  X-4  is  an  unmanned  helicopter  with  four  rotors  combined 
with  a  cross  scheme.  The  quadrotor  generates  its  lift  by  these  four  rotors.  The  two 
opposing  rotors  form  one  pair,  where  the  first  pair  (#1  and  #3)  is  set  on  the  x-axis  and  is 
rotated  counter  clockwise  while  the  second  one  (#2  and  #4)  is  set  on  the  y-axis,  rotated 
counterclockwise  as  shown  in  Figure  34. 


Pitch  Roll 


Figure  33.  Quadrotor  Configuration  Scheme 

The  earth-fixed  inertial  frame  (Xu,  Yu,  Zu )  shown,  specify  the  location  of  the 
vehicle,  while  the  body  frame  (Xb,  Yb,  Zb)  specify  the  vehicle  orientation,  together  with 
angles  of  rotation,  roll  (cjY),  pith  (9),  and  yaw  (i //). 

Let  the  mass  of  the  quadrotor  be  m  that  represents  the  whole  structure  mass  of  the 
quadrotor,  as  it  stated  in  the  assumption  (d)  in  the  previous  section. 
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Let  BV  the  velocity  vector  of  the  vehicle  and 11  co  the  rate  of  change  of  the  angle 
(angular  velocity)  in  body  frame,  respectively,  "x  the  position  of  the  vehicle  in  the 
inertial  system 


3V  =  ui  +  vj  +  wk  = 


5  a>  =  pi  +  qj  +  rk  = 


u 

v 

w 

p 

q 

r 


(1) 


(2) 


“x  =  xi  +  yj  +  zk  = 


(3) 


Also,  the  Euler  angles  of  the  vehicle  which  rotates  around  the  XYZ  Body  frame 
axis  are  represented  as 


A  = 


e 

¥ 


(4) 


4.  Transformation  Matrices 


In  order  to  transfonn  the  state  vectors  from  {U}  frame  to  the  {B}  frame,  a 
transfonnation  matrix  must  be  used.  This  matrix  according  to  the  convention  of  rotate 
first  around  x-axis,  then  around  y  axis  and  finally  by  the  z  axis  as  described  in  [1]  can  be 
found  by  the  following  equation: 


B 

u 


R  =  R 


X,(f 


'-R.. 


:R 


z,y/ 


(5) 


where RX^,RV0,RZV,  are  the  rotation  matrices  around  each  axis,  respectively,  as 
follows: 
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Rotation  around  x-axis:  Rx#  = 


10  0 
0  cos^  sin^ 
0  -sin^  costfi 


(6) 


with  yaw  angle  yr  < 


f  71  7T^ 


2  2 


Rotation  around  y-axis:  Rv  e  = 


cos  9 

0 

-sin  9 

0 

1 

0 

sin  9 

0 

cos  9 

(7) 


with  pitch  angle  9  • 


f  71  71^ 


2  2 


Rotation  around  z-axis:  Rz  = 


cos^  sin^  0 
-sin^  cos^  0 
0  0  1 


(8) 


with  roll  angle  (p  < 


f  71  7T^ 
2  ’  2 


So,  if  we  use  the  Eq.  6,  7  and  8  in  Eq.  3.5,  the  final  form  of  the  transformation 
matrix  can  be  derived: 


BuR  =  K,t*Ry,e*R^= 


cos  9  cos  y/ 

sin  (f)  sin  9  cos  y/  -  cos  <f>  sin  y/ 
cos  <f>  sin  6  cos  y/  +  sin  <f>  sin  yr 


sin  yr  cos  6 

-  sin  (j)  sin  9  sin  y/  +  cos  <p  cos  y/ 
cos  (f>  sin  9  sin  y/  -  sin  <p  cos  y/ 


-sin  9 
sin  (j)  cos  9 
cos  </>  cos  9 


(9) 


The  transformation  matrix  from  {B}  to  the  {U}  coordinate  frame  is: 


u 

B 


R  = 


cos  9  cos  y/ 
cos  9  sin  y/ 
sin  9 


-  cos  (f)  sin  y/  +  sin  (j)  sin  9  sin  y/ 
cos  <f>  cos  y/  +  sin  <j>  sin  9  sin  yr 
sin  (p  cos  9 


sin<f>  sin  y/  +  cos  (f>  sin  9  cos  yr 
-  sin  (f>  cos  yr  +  cos  <f>  sin  9  sin  yr 
cos  <f>  cos  9 


(10) 
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5.  Kinematic  Equations 

Having  defined  the  transformation  matrices,  we  can  now  find  the  velocity  of  the 
vehicle  in  the  inertial  frame  instead  of  the  Body  coordinate  frame  and  it  can  be  given  by: 


X 

u 

“x  = 

y 

=  ubRbV=  l‘R 

V 

z 

w 

(cos  9  cos  y/)u  +  (-  cos  cp  sin  y/  +  sin  cp  sin  9  sin  y/)v  +  (sirup  sin  yr  +  cos  cp  sin  9  cos  y/)w 
(cos  9  sin  y/)u  +  (cos  cp  cos  y/  +  sin  (p  sin  9  sin  y/)v  +  (-  sin  cp  cos  y/  +  cos  (p  sin  9  sin  y/)w 
(sin  0)u  +  (sin  cp  cos  9)v  +  (cos  cp  cos  9)w 


(3.11) 


Moreover,  the  rate  of  change  of  the  Euler  angles  of  the  vehicle  as  stated  in  [1]  can 
be  found  by: 


A  = 


e 

'/> 


Q-\A)bco 


(12) 


where  the  matrix  Q(A)  and  its  inverse  one,  Q  '(A)  are  given  by  the  equation: 


0(A)  = 


1 

0 

0 


0  -sin  <9 
cos  (f>  sin  (f>  cos  9 
-sin^  cos  (f)  cos  6 


Q\  A) 


1  sin  (j)  tan  6  cos  (f)  tan  6 
0  cos  ^  -sin^ 

0  sin  (f)  /  cos  6  cos  ^  /  cos  6 


(13) 


(14) 


So,  using  the  Eq.  2  in  Eq.  12  the  following  equation  can  be  derived: 


“1 

sin  (f)  tan  6 

cos  (j)  tan  9 

P 

9 

= 

0 

COS  (f) 

-siruf) 

q 

(15) 

y\ 

0 

sin  (j) !  cos  9 

cos  (f) !  cos  9 

r 

The  last  equation  and  Eq.  1 1  are  the  kinematic  equations  of  the  quadrotor. 
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6.  Dynamic  Equations 

According  to  Newton’s  second  law  in  the  {U}  orientation  frame,  the  following 
equation  represents  the  applied  force  UF  and  the  acceleration  of  the  vehicle  ux  in  the 
{U}  frame: 


1 

"51 

5-!  J 

_ 1 

t 

X 

1 

bC  k," 
_ 1 

=  — (in  V )  =  in  —  V  =  m  x  =  m 
dt  dt 

1 

i _ 

(16) 


If  we  multiply  by  both  sides  the  transformation  matrix  B  R  of  Eq.  9,  Eq.  16  will  be 


transfonned  to: 


V  fR“F  =  YbF  =BRm“x  =  mBuR 

u  u  u 


(17) 


BF=mBR  —  Cx) 
u  dt 


(18) 


Continuing  the  math: 

E  Bp  =m uR  Jt  ( BR BV)  =  m Br(bR BV  +  BR BV)  =  mCuR IR BV  +  *UR  “R BV)  => 

E  bF  =m(BV  +  bR ;RbV)  =  m(BV  +  BR[gRS(Ba>)]x  BV)  => 

£BF  =  m(BV  +  S(Ba>)xBV ))  (19) 


Since BR  =  “RS(Ba>) ,  where  S(Ba>)  is  a  skew  symmetric  matrix.  The  term 
S(Bco)x  BV  is  the  Coriolis  term  and  can  be  written  as: 


S{Bco)xVB  =  Ba>xVB 


qw  -  rv 
ru  -  pw 
pv  -  qu 


(20) 
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If  we  substitute  the  derivative  of  Eq.  1  and  the  Eq.  20  into  Eq.  19  and  rearrange 
the  terms,  then: 


ii 

qw  -  rv 

)=>- 

1 

_ 1 

u 

qw-rv 

X Bp  =  m( 

V 

+ 

ru  -  pw 

Fy 

= 

V 

+ 

ru  -  pw 

w 

pv  -  qu 

m 

Fz  J 

w 

pv  -  qu 

=> 

u 

V 

1 

1 _ 

qw-rv 

ru  -  pw 

w 

in 

Fz\ 

pv  -  qu 

(21) 


where  Fx,Fy,F_  are  the  forces  in  the  body  coordinate  frame  in  each  axis,  u,v,w 
and  u,v,w  are  the  velocity  and  the  acceleration  components  in  the  body  frame, 
respectively,  in  each  axis  and  p,q,rarQ  the  angular  velocity  components  in  the  body 
frame  in  each  axis. 

Using  the  Euler’s  Law,  the  momentum "M0  in  the  inertia  frame  will  be  given  by: 

'M()=^-CA)  =  ^-(“RJh  bF>)  = 
dt  dt 

;rjb  Bcb  +  ;rjb  bf> + =  ;r{bg>xjb  Bo>) + ;rjb  bf>  => 

=>  x  BUR  “Mo  =  BuR[uBR(BaxJBBm+  buR[  bRJb  Bj>]  => 


L 

M 

N 


=  (bcoxJb  bo>)  +  Jb  Bcb 


(22) 


where  L,  M,  and  N  rolling,  pitching  and  yawing  moments,  respectively,  in  the 
body  coordinate  system,  B  A  the  angular  momentum  in  the  Body  frame  and  JB  is  the 


Inertia  Tensor  of  the  quadrotor  in  the  B  coordination  frame  (JB 


>3x3 


>0): 


JB  = 


J  J  J 

xx  xy  xz 
Jyz  Jyy  J yz 

\  ^  zx  ^  zy  ^  zz  J 


(23) 
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Then,  the  angular  acceleration  will  be  given  by  the  equation: 


(24) 


where 


T 

x: 

0 

0 


0 


0 


0 


0 


(25) 


since  J  =  J  =  J  =  J  =  J  =  J  =0  because  the  quadrotor  is  symmetric  about  all 

xy  yz  xz  zx  yx  zy  l  J 

three  axes. 

If  we  take  the  derivative  of  the  angular  velocity  in  Eq.  2: 


B 


CO 


P 

q 


In 

And  combine  Eq.  2  with  Eq.  23  to  take  the  external  product,  we  will  have: 


P 

J  v  —  J  q  —  J  r 

xx  Jr  xyot  xz 

bcox.Jb  b  g)  = 

q 

X 

-JyXP+J}yq-JyP' 

r 

-Jzxp-Jzyq  +  Jzzr 

-r(-Jyxp  +  Jyyq  -  Jyzr )  +  q{—J-xP  ~  J -  J) 

nixxp  ~  J xyq  ~  Jxzr )  -  p(~Jxxp  ~  Jzyq  +  Jzzr) 
~q(JxxP  ~  Jxyq  ~  Jxzr)  +  P(-Jyxp  +  J yyq  -  Jyzr ) 


(26) 


(27) 


Finally,  if  we  substitute  Eq.  3.31  and  Eq.3.33  in  equation  3.30  we  will  have: 


p 

'  L 

-ri-JyxP  +  Jyyq  ~  J y/)  +  qi~J zxP  ~  J ^  J -J~) 

q 

=  JB-\ 

M 

- 

f(JxxP  -  Jaq  -  Jn)  -  P(-Jzxp  -  J :yq  +  Jzzr) 

r 

N 

-q(Jap  -  J„q  -  Jx:r) + pi-JycP + J yy  q  -  Jy/) 
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(28) 


The  last  equation  together  and  the  Eq.  21  are  the  dynamic  equations  of  the 
quadrotor.  Combining  the  kinematic  and  the  dynamic  equations  of  the  quadrotor  you  end 
up  with  the  nonlinear  differential  equations  of  motion  of  a  rigid  body  with  six  degrees  of 
freedom. 


7.  Forces  Calculation 

a.  Gravity  Force 

Gravity  FG  is  the  main  force  acting  on  the  quadrotor  in  the  earth  fixed 

frame  along  z-axis  toward  the  ground,  which  is  given  by  the  total  mass  of  the  quadrotor 
multiplied  by  the  acceleration  of  the  gravity  as  follows: 

“  0  " 

“Fg  =  0  (29) 

mg_ 

where  m  is  the  total  mass  of  the  vehicle,  g  is  the  acceleration  of  the  gravity. 
Consequently,  Eq.  29  will  be  transformed  in  the  body  coordination  frame  using  the 
transformation  matrix  BuR  from  Eq.  9: 

0  -mg  sin  6  FGx 

BFG  =  BR  0  =  mg  sin  cp  cos  6  =  FGy  (30) 

mg  mg  cos  (p  cos  6  FG_ 

where  BFG  is  the  gravitational  force  in  the  body  frame  and  FGl,FG2,FG3  are  the 
components  of  the  gravity  force  in  the  jc,  y  ,  and  z  direction,  respectively. 
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b.  Drag  Force 

The  second  force  acting  on  the  quadrotor  is  the  drag  force  that  opposes  the 
movement  of  the  quadrotor  and  is  equal  to  the  coefficient  of  drag  multiplied  by  the 
square  of  the  velocity  in  every  axis.  In  the  inertial  frame  the  drag  force  is  given  by  the 

\caAep  X2 

equation:  “FD=  p'V2  (31) 

\cdap-v:2 

where  A  is  the  cross-sectional  area,  p  is  the  air  density,  Vx ,  Vv ,  V .  are  the  body  frame 
velocity  components  in  the  x,  y,  and  z  axis,  respectively.  CDx ,  CDy ,  C/Jz  are  the  drag 
coefficients  in  each  axis.  In  order  to  get  the  drag  force  components  FDx,FDy,FDz  in  the  x, 
v,  and z  direction  in  the  body  frame,  we  will  also  use  the  matrix BuR  from  Eq.  9. 

X~CDxAcpx- 

jcaJrpy2 

\caAcPe 

However,  the  velocity  regime  in  which  the  quadrotor  operates  is  at  slow 
airspeeds.  Avoiding  the  complex  calculation  of  computational  fluid  dynamics  and  using 
the  same  considerations  as  in  [1],  where  the  coefficients  of  drag  varies  with  the  object’s 
shape  about  ( CDx  =  CDy  =  CD_  =1.12)  and  the  cross-sectional  area  facing  each  axis  and 

the  combined  surface  area  of  the  cross-frame  with  batteries  and  motors,  are  considered, 
then  Ac  is  found  to  be  about  0.16m  for  the  x  and  y  axis  and  0.4m  for  the  z-axis, 

respectively.  Also,  by  using  as  the  common  air  density  value  of  977 .2  Kg  /  m3 ,  the 
resulting  drag  force  components  are  found  to  be  around 
FDx  =  8.756 *  10  5  * i2 Kg  /  m, =  8 .756  *  1 0  5  * yKg  I  m,FDz=  5.289 *  1  O'4  * i2Kg  /  m  . 
This  is  quite  negligible  and  thus  can  be  ignored. 
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c.  Thrust  forces 

The  quadrotor’s  motion  is  only  controlled  by  varying  independently  the 
speed  of  the  four  rotors.  Each  rotor  produces  both  a  thrust  and  a  torque  (moment)  about 
its  center  of  rotation.  Let  FT  .  be  the  thrust  for  its  rotor,  respectively,  and  /  the  distance 

of  each  rotor  from  the  center  of  the  quadrotor’s  mass.  The  quadrotor’s  motion  is  affected 
by  the  four  propellers  only  in  the  z-direction  so  we  have  that: 


0 

1 

J?  J 

_ 1 

II 

0 

= 

Fry 

Ft i  Fj2  +  Ft3  +  Ft4  J 

1 

_ 1 

(33) 


where  Fn,FT2,FT3,FT4  are  the  thrusts  that  the  1st,  2nd,  3rd  and  4th  rotor  produces, 
respectively,  and  FTx,FTy,FTz  are  the  total  thrust  force  in  each  direction  in  body  frame. 


d. 


Total  Force 


The  total  force  is  the  sum  of  the  gravity,  drag  and  thrust  forces: 


Z‘F= 


F  +F  +F 
F  +F  +F 

rDy  ^  rTy  ^  r  Gy 

F  +  p  +  F 

1  Dz^  1  Tz^  1  Gz 


-mg  sin  6 
mg  sin  <p  cos  6 
mg  cos  cp  cos  6 


1 


C-Dx^cP* 


\Co-Api1 


+ 


0 

0 

Fj,^  +  Fj2  Fj^  ■+■  Fj 4 


Fx 

Fy 

F. 


1 

-mg  sin  6*  +  —  CDxAcpx~ 


1 


•  2 


mg  sin  (p  cos  0  +  -  CDyAcpy 

1 

mg  cos  (p  cos  0  +  —  CDzAcpz  +  (Fn  +  FT2  +  Fn  +  FTA  ) 


(34) 


56 


8.  Moments  (Torques)  Calculation 

Let  zi  be  the  torque  generated  by  each  propeller.  The  rolling  moment  L(rroll)  is 
produced  by  varying  the  right  (#2)  and  the  left  (#4)  rotor  speeds 

L  =  ^  roii  =r2-r4  =  Frl-  FTl  =  (Frj  -  FTf  )l  =  (Fright  -  Fleft)l 

Similarly  the  pitching  moment  M(jpitch)  (torque)  is  produced  by  varying  the  front 
(#1)  and  the  back  rotor  (#3)  speeds 

M  =  r pUch  =L  -r3  =  Frl- FTl  =  (Fr_  -  FT} )l  =  (Ffront  - Fhack)l 

Due  to  the  third  Newton’s  law,  the  drag  of  the  propellers  produces  a  yawing 
moment  (torque)  on  the  body  of  the  quadrotor.  Therefore,  the  total  yawing  moment  is 
obtained  from  all  the  rotor  speeds  (clockwise  and  counterclockwise),  is  in  opposite 
direction  of  the  motion  of  the  propellers  and  is  given  by 

N  =  tyaw  =  (. FTi  +  FT}  -  FTi  -  FTi  )d  =  (Fright  +  Fleft  -  Ffront  -  Fback)d 

where  d  is  the  force  to  moment  scaling  factor  calculated  to  be  d=4N.m. 

9.  Moments  of  Inertia  Calculations 

a.  Moment  of  Inertia  along  x 

If  the  two  motors  (#1  and  #3)  along  x  axis  and  the  main,  central  body  of 
the  quadrotor  considered  cylindrical  in  shape  with  mass  mt ,  m3  and  m  ,  radius 
Rt , R3  and  Rr  and  height /?,,/?,  and  h,  ,  respectively,  as  shown  in  Figure  34. 


z-axis 


Figure  34.  Moments  of  inertia  about  x,  y  and  z-axis. 
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Only  the  rolling  motion  is  considered.  So,  the  moment  of  inertia  about  the 
x  axis  would  be  due  to  the  main  body  and  the  motors  #1  and  #3  motion  about  x-axis  and 
due  the  motion  of  the  other  two  motors  (#2  and  #4)  about  x  axis 

Jxx=Jc+J\+Ji+JiA  (38) 


The  moment  of  inertia  of  a  cylinder  about  an  axis  perpendicular  to  its 
body,  as  specified  in  Halliday-  Resnick- Walker  [57],  thus  the  moment  of  inertia  due  to 
main  body  and  the  two  motors  are 


J  _mc*  rc  ,  mc  *  K2 

o,x  4  12 


T  m,*r, 2  m,*h,2 

Jlx=— - ~ +  ~ - — 

4  12 


m , *  r '  m , *  hr 

J}x  =— - ~ +  ~ - L 

’  4  12 


(39) 

(40) 

(41) 


The  moment  of  inertia  of  two  identical  spheres  connected  together  by  a 
horizontal  arm,  and  rotating  about  a  vertical  axis,  which  is  passing  through  the  center  of 
the  ann  and  is  perpendicular  to  it,  as  also  specified  in  [57],  thus  the  moment  of  inertia 
due  to  motors  #2  and  #4  rotating  about  x-axis  approximated  to  be 

J 2-4, x  =  2mrl2  (42) 


Since  the  motors  are  identical: 


ml  =  m,  =  m2  =  m4  =  mr 


i]  =  /;  =  r2  =  rA  =  rr  and  \  =h3=h2=  h4  =  hr 


is 


Finally,  the  total  inertia  about  x  axis  by  substituting  Eq.  39  to  42  in  Eq.  38 


J 


mr,2  ,  mh,2  |  marc2  |  mji c2 

2  6  4  12 


+  2m,./2 


(43) 
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b.  Moment  of  Inertia  along  x 

Similarly,  because  the  quadrotor  structure  is  symmetrical  and  the  motors 
are  identical  the  moment  of  inertia  about  y-axis  would  be  exactly  the  same: 


J 


yy 


mr 2  mh  2  m  r  2  m  h  2 

r  _| _ r  _| _ o  c  _| _ o  c 

2  6  4  12 


+  2  mrl2 


(44) 


c.  Moment  of  Inertia  about  z-axis 

The  moment  of  inertia  about  z-axis,  will  be  due  to  main  body  and  due  to 
all  the  motors.  The  moment  of  inertia  due  to  the  main  body  is  found  in  [57]  to  be: 


J 


c,z 


mcrc 

2 


(45) 


The  moment  of  inertia  due  to  all  the  motors  #1  to  #4  is  approximated  to 
be: 


4 


2-3-4,  z 


—  Ami 2 


(46) 


Therefore,  the  total  inertia  about  z-axis  is: 


J  = 


mr 


■  +  4ml 2 


(47) 


All  the  above  moments  of  inertia  were  calculated  for  the  quadrotor  and 
found  to  be  as  follows: 


Jxx  =  0.0  3Kgm2 

J yy  -  0. 03 Kgm2  .  (48) 

,J__  =  0.04  Kgm2 
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10.  Final  Equations  of  Motion 

Combining  the  Eq.34  with  the  dynamic  Eq.  2 1  we  can  get: 


1  , 

-mg  sm  9 +  -CDxAcpx 

u 

1 

m 

Z 

1  2 

mg  sm  (p  cos  9  +  —  C DyAcpy 

qw—rv 

=> 

V 

. 

- 

ru  -  pw 

w 

1 

mg  cos  cp  cos  9  +  —  CD,Acpz  +  (Fr  +  Fr  +FT  +  Fr  ) 

2  12  3  4 

pv  -  qu 

u 

v 

w 


1 

2  m 

1 

2  m 


-g  sin  9  +  —  Cl)xAcpx  +  (rv  -  qw ) 


g  sin  <p  cos  9  +  - —  CDyAcpy 2  +  ( pw  - ru ) 


1  2  1 

gcos^>cos<9  + - CD.Acpz  +  —  (Ft  +Ft  +  Ft  +  Ft)  + qu- pv 

2m  "  m  1  2  3  4  ■ 


(49) 


If  we  neglect  the  drag  force  then  the  equation  takes  the  fonn: 


u 

v 

w 


-g  sin  9  +  (rv  -  qw) 
gsin^>cos#  +  (  pw  -  ru  ) 

g  cos  cp  cos  9  +  —  (Ft  +Ft  +Ft  +Ft)  +  qu  -  pv 

mi  1234 


(50) 


Similarly  as  before,  if  Eq.  17  is  combined  with  Eq.34  and  the  transformation 
matrix  in  Eq.  9  is  applied,  then: 
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—  (FT  +Ft  +Ft  +  Ft  )(cos0sin^  +  cos0sin$cos^) 

m  1  2  3  4 

—  (Ft  +Ft  +Ft  +  Ft  )(-sin0cos^ +  cos^sin#sin^/r) 

m  1  2  3  4 

—  ( Ft  +Ft  +Ft  +Ft)  cos  (f)  cos  6  -  g 

m  1  2  3  4 


(51) 


Substituting  Eqs.  35,  36  and  37  in  Eq.  28  becomes: 
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Equations  15,  51  and  52  form  the  dynamical  model  of  the  quadrotor  that  will  be 
used  for  the  design  of  the  controller.  Although  some  approximations  and  assumptions 
already  were  made,  some  more  simplifications  have  to  be  done  for  the  better 
implementation  of  the  control  scheme. 
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B.  MODELLING  OL  GROUND  ROBOT  QBOT 


1.  Differential  Drive  Kinematics 

The  notation  and  a  mathematical  model  for  the  kinematics  of  a  wheeled  mobile 
robot  will  be  developed  in  this  section  complying  with  references  [56]-[61].  Wheeled 
robots  have  some  simplifying  features  that  make  them  easier  to  be  modeled  than  real 
vehicles,  since  the  operation  field  is  on  2-D  space  environment.  In  particular,  these  robots 
have  two  independently  driven,  coaxial  wheels.  The  speed  difference  between  the  two 
wheels  causes  a  rotation  of  the  robot  about  the  center  of  the  axis  while  the  wheels 
produce  motion  in  the  forward  or  reverse  direction.  It  is  known  that  real  vehicle 
dynamics,  at  high  velocities,  cause  a  vertical  motion  which  is  compensated  for  by 
applying  suspension  to  their  design.  However,  because  these  robots  operate  in  very  low 
speeds  the  vertical  motion  is  almost  negligible  and  no  suspension  is  required. 

Since  the  robot  is  moving  on  a  horizontal  plane,  the  degrees  of  freedom  are  three 
(3  DOF),  the  x  and  y  positions  along  the  axis  of  the  differential  drive  and  the  rotation 
around  z-axis: 


B 


x 


p  = 


B3 


(53) 


The  following  figure  34  depicts  the  kinematics  of  the  iRobot  Create,  where  the 
distance  between  the  two  wheels’  centers  is  L=0.34m. 


1 - *  X 

Figure  35.  Kinematics  of  the  differential  drive  robot,  iRobot  Create. 
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As  described  before,  in  the  case  of  the  quadrotor,  the  motion  has  to  be 
transfonned  from  the  body  (robot  here)  frame  to  the  inertial  frame.  The  transformation 
matrix  of  Eq.9  will  be  “rotation  around  the  z-axis”  since  we  are  dealing  only  with  a  2-D 
frame: 


u 

B 


R,  = 


cos  <9 
-sin  5 
0 


sin  5 
cos  5 
0 


0 

0 

1 


So,  Eq.53  becomes: 
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9 
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u3  =  3 


(54) 


If  the  position  of  its  wheels’  centers  areC;  ,  *  =  1, 2  for  right  and  left 
wheel,  respectively,  the  longitudinal  component  of  velocity  is  v,  .  and  in  lateral 
direction  is  vlati ,  by  differentiating  Eq.  53the  velocity  of  C;  can  be  obtained  in  the  body 
frame  (2x1  vector): 
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(55) 


63 


From  body  frame  to  the  inertia  frame: 
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=  ubRzbv=ubR 
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x  cos  3  +  y  sin  3 
-jcsin^  +  y  cos  <9 


(56) 


Consider  the  rolling  wheel  with  a  point  of  contact  P  with  the  ground. 


Figure  36.  Top  View  of  the  Robot 

Because  it  is  rolling,  the  velocity  of  point  P  is  zero.  Suppose  the  wheel 
rolls  along  the  y  axis  on  the  y-z  plane  given  by  x  =  0.  If  we  recall  from  physics  [57]  that 
the  velocities  of  two  points  A  and  B  on  a  rigid  body  with  angular  velocity  go,  are  related 
by  the  equation: 

vB  =vA  +a>xAB 
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Then  the  velocity  of  the  center  C;  can  be  obtained  from  the  previous 

equation: 

vc. ,  =  Vp  +  (Oi  x  PCI  (57) 

If  (pt,i  =  1,2  each  wheel’s  speed,  respectively,  then  the  velocity  of  the  center  Ct 
will  be  ( i ,  j ,  k  the  unit  vectors  along  x,  y,  z  axis,  respectively): 

vc,i=(°ixri=<Pii  x  rk  =  -r<pj  (58) 

Since  the  robot  cannot  slide  in  a  lateral  direction,  the  velocity  of  the  point 
Ci  must  be  along  the  longitudinal,  thus  the  velocity  of  the  point  C(  in  the  lateral  direction 
must  be  zero: 

vlat  i  =  0  =>  -xi  sin  3  +  y,  cos  <9  =  0 
After  rearranging  terms: 

x,  sin  9  =  j>  cos  9  x,  tan  9  =  j>  (59) 

Then  the  Eq.  65  will  be  equal  to: 

Vlong,i  =  ~r(Pi  (6°) 

V<  =  0 

Next,  (pt  is  measured  in  a  counterclockwise  direction  from  one  arbitrarily 
chosen  side  of  the  robot  as 


Figure  37.  The  wheel  angles  (Positive  in  a  counterclockwisedirection  from  one 
(arbitrarily  chosen)  side  of  the  mobile  robot).  After  [58]. 
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Now  consider  the  coordinates  of  the  center  of  the  axle  (x,  y)  which  is 
clearlyhalf  way  between  C,  and  C2 : 


x,  +  x, 
x  =  — - - 

2  (61) 
v  =  Zl±Zi 
2 

The  velocity  of  the  point  Ci  is  given  by: 


VC,i  = 


Xj  +x2 
2 

T1+T2 


(62) 


The  longitudinal  velocity  component  if  we  combine  Eq.56,  60  and  62will 
become: 


vbng,i  =xcos$+ysin$: 


x,  cos  <9  +  j>j  sin  <9  x2  cos  <9  +  y2  sin  <9  _vc,i+v< 


■  + 


C,  2 


r<Pi+r<p2 


v long, i 


(63) 


Now  if  we  consider  the  two  points  Q  and  C2  which  are  rigidly 

attached  to  theaxle  and  the  robot,  like  Eq.  57,  the  velocities  of  these  two  points  are  related 
by  theequation: 

E'2  =  vc,  +  &k  x  C,  C2  (64) 

That  can  be  transformed  to: 


-r(p2  =  - r<px  +  L3  =^>  L3  =  rcpx  -  rep <2  ^>3=  J— — 

L 

If  the  velocities  of  the  left  and  right  wheel  are  denoted  as: 

%  =  (Prigh,  md  <Pl  =  <Plefi 


VC,  =  bright  and  VC2  =  Vleft 


(65) 
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we  have  the  two  equations  that  relate  the  robot  velocities  tothe  the  wheels’  speeds: 


bright  +r(Pl4t 


v long, i 


(66) 


q  r(P right  left 

L 

If  we  substitute  Eq.59into  Eq.57: 


(67) 
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(68) 


Also  sin  ce  y  =  x  tan  9  =  v,  . 


sin  9 


■  y  =  Vlong,i 


sin  .9 


(69) 


Finally,  if  we  substitute  the  Eq.63  into  the  state  equations  of  Eq.54,  they  can  be  written  in 
the  inertia  frame  as: 
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The  velocities  of  the  wheels  are  bounded  such  that: 


—V  <  V,  ,  <  V 
max  lejt  max 


and  —v  <  v  ,  <  v 

Kmax  v right  Kmax 


(71) 


where  =  0.5 ml s 

The  robot  will  move  forward  when  v  . ht  >  vleft  >  0 ,  left  when  vleft  >  vright  >  0  and 
will  spin  in  place  when  vlefl  =  vright ,  and  not  equal  to  zero. 
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Returning  to  the  definition  of  a  two  wheel  differential  drive  robot,  it  has  two  drive 
wheels  mounted  on  a  common  axis  and  each  wheel  is  controlled  by  a  different  motor. 
Therefore,  each  wheel  is  able  to  be  driven  at  different  velocities  either  forward  or 
backward.  As  it  is  already  reported  before,  by  varying  the  velocity  of  each  wheel,  rolling 
motion  can  be  achieved  and  the  robot  will  rotate  about  a  point  that  lies  along  the  common 
left  and  right  wheel  axis.  This  point  is  widely  known  as  the  ICC  -  Instantaneous  Center  of 
Curvature  [58]  (See  Figure  37). 

x 


^ . ► 

L  /2 


R 


Figure  38.  Kinematics  of  differential  robots.  After  [58], 

The  trajectory  of  the  robot  can  be  controlled  by  varying  the  velocities  of  the 
wheels.  The  rate  of  rotation  co  about  the  ICC  must  be  the  same  for  both  wheels.  Hence, 
the  following  equations  establish  a  relation  between  the  motion  parameters  of  a 
differential  drive  mobile  robot. 

%,»,  =<»(*  +  f)  (72) 

f)  <73) 

where  L  is  the  distance  between  the  centers  of  the  two  wheels,  vright,vleft  are  the  right  and 

left  wheel  velocities  along  the  ground  ,  and  R  is  the  signed  distance  from  the  ICC  to  the 
midpoint  between  the  wheels. 
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The  Eq.72  and  73  can  be  solved  at  any  instance  of  time  for  R  and  co  as  follows: 


L(  Vrigh,+Vleft) 
bright  ~Vleft) 

(74) 

V  ,  —V,r 
right  left 

L 

(75) 

Figure  37  assumes  that  the  robot  is  at  some  position  (x,  y)  and  is  heading  in  a 
direction  making  an  angle  9 with  the  X  axis.  Knowing  velocity  vrj  ht,vleft ,  and  using 

Eq.74,  75,  the  ICC  location  {{ICC x, ICC y)  is  determined  as: 
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x  -  R  sin  3  =  x - ^  r,ght - —  sin  <9 

2iV right  ~Vleft) 

(76) 

y  +  Rcosi9  =  y+  ^  "sh‘ - '—cosS 

2(V right  ~V left ) 

(77) 

2.  Forward  Kinematics 

The  forward  kinematics  problem  of  a  mobile  robot  for  given  wheel  velocities  and 
initial  robot's  configuration  (x,  y,  S)t=0  is  the  determination  of  the  robot's  position 

{x,y,  &)t  at  any  other  time  t  [56]. 


a.  Robot's  Position  Relative  to  ICC  Location 

If  non-varying  velocities  vnyhl ,  vlefl  are  given,  the  ICC  location 
{ICC  x, ICC  v)  will  be  a  fixed  position.  Hence,  the  robot’s  state  at  time  t  +  At  can  be 
expressed  via  its  state  at  time  t  as  follows: 
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where  (x,y,  3)  and  (x  \  v <9 ')  are  the  robot's  positions  at  time  t  and  t  +  At  respectively. 
This  equation  describes  the  motion  of  a  robot  rotating  about  its  ICC  with  a  radius  of 
curvature  R  and  an  angular  velocity  co  (see  Figure  40). 

^  P(t+at) 


4  p«) 


Figure  39.  Forward  kinematics  relative  to  ICC.  After  [58]. 

b.  Robot's  Position  Relative  to  its  Initial  Position 

General  motion  equations  of  the  robot  capable  of  moving  is  in  a  particular 
direction  3{t)  at  a  given  velocity  v(t)  are  described  as  follows: 

t 

x(t)  =  |  v(t)cos[$(t)]dt 
0 
t 

y(t )  =  |  v(t)  sin[^(t)]Jt  (79) 

0 
t 

3(t)  =  J  (o(t)dt 

o 

For  a  differential  drive  robot,  such  as  the  Qbot,  Eq.  79  becomes: 

1  r 

*(0  =  -  J  bright  (0  +  Vleft  (0]  COS[5(0]* 

^  0 

y(  0  =  \  J  \yright  (0  +  vleft  (0]  sin[5(0]*  (80) 

*9(0  =  y}[vngfe(0-vfe/,(0]^ 

If  the  Eq.79  is  simplified,  the  robot's  position  at  time  t  +  At  will  be: 
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x'  x  +  vcos[i9(t)]<7i 

y'  =  y  +  vsin[i9(t)]c>i  (81) 

S'  1 9  +  coSt 

Similarly,  the  Eq.  80  will  become: 

^  ^  t  ^ right  (t)  +  vleft(t)\co?,[3(t)}St 

y  +  ~2  bright  (0  +  vieft  (0>  in 

<9  +  jb'n,Jt)-vleft(t)]St 
For  the  special  cases  of  vright  =  vleft  =  v  and  -vright  =  vleft  =  v  in  Eq. 81,  we 

will  have 

x  +  v  cos  38t 

y  +  vsxn&8t  (83) 

x 

V  ,  (84) 

,9  +  -2v* 

L  J 

respectively. 
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C.  CONTROL  ARCHITECTURE  OF  THE  TWO  MODELS 

Now  that  kinematics  /  dynamics  for  both  vehicles  have  been  developed,  we  can 
elaborate  on  Figures  30  and  31  and  present  overall  feedback  control  architecture  (Figure 
41). 


NaturalPoint2  NaturalPomtlO  NaturalPointl  MaturalPoint3 


QUADROTOR  QBALL-X4 


CONTROLLERS 


NaturalPoint6 


GROUND  ROBOT  QBOT 


Natural  Point?  Natural  Point9  NaturalPoint8  NaturalPoint5 


Figure  40.  Control  Architecture 

Figures  42  and  43  provide  more  details  about  the  robot  plant  models  and  explain 
graphically  the  derivation  of  the  equations  of  motion  and  how  it  will  be  implemented  in 
the  Simulink  development  environment. 
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Figure  4 1 .  Quadrotor  QBall-X4  Dynamics  Plant 
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QBOT  KINEMATICS  MODEL 
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Figure  42.  Ground  Robot  Qbot  Kinematics  Representation 
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IV.  CONTROL  STRATEGY 


A.  INTRODUCTION 

The  dynamic  model  of  the  quadrotor  has  been  presented  in  many  papers,  so  with 
some  small  variations,  concerning  the  different  assumptions  to  be  done  or  the 
environment,  it  will  be  the  same.  However,  the  control  schemes  can  be  different, 
choosing  from  feedback  linearization  control,  backstepping  control,  visual  control  and 
direct  methods  with  inverse  dynamics.  In  this  chapter,  the  general  control  scheme  will  be 
presented.  The  first  controller  is  the  implemented  one  with  the  Linear  Quadratic 
Regulator  method  imposed  in  five  different  channels,  roll,  pitch,  x-y  position,  yaw  and 
height.  Although,  the  modeling  took  place  for  a  nonlinear  model,  the  nature  of  the 
experiments  (laboratory  -  slow  speeds)  allows  us  to  use  a  linearized  model  for  the 
quadrotor  control.  Secondly,  another  controller  is  proposed  using  inverse  dynamics  in 
Virtual  domain  (IDVD)  control  technique  for  achieving  a  quasi-optimal  solution  in  real 
time  (tenths  of  second).  This  controller  requires  creating  a  trajectory  generator  with 
certain  characteristics  that  will  be  described  below  and  follows  the  quadrotor  dynamics. 
Unfortunately,  this  controller  was  not  implemented  and  tested  for  this  model  but  it 
provides  the  steps  and  so  far  implementation  for  future  work  within  the  students  already 
or  starting  working  in  this  project. 

B.  IMPLEMENTED  CONTROLLERS 

1.  Control  Inputs 

The  quadrotor  is  controlled  by  independently  varying  the  speed  of  the  four  rotors. 
For  every  attitude  change  the  angular  velocity  of  motors  is  changed,  but  the  total  thrust  of 
all  the  four  motors  is  constant  in  order  to  maintain  the  height.  So,  in  order  to  produce  a 
roll  angle  ( </>  )  along  the  axis  XB  ,the  angular  velocity  of  the  motor  #2  is  increased  and  the 
angular  velocity  of  motor  #4  must  be  decreased.  Likewise,  in  order  to  produce  a  pitch 
angle  (0)  along  the  axis  YB  ,  the  angular  velocity  of  the  motors  #3  must  be  increased,  and 
the  angular  velocity  of  motor  #1  must  be  decreased  while  at  the  same  time  the  thrust 
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must  be  kept  constant.  It  can  be  understood  that  yaw  motion  along  the  Z-axis  of  the  body 
frame  will  be  implemented  by  increasing  total  angular  velocity  of  motors  (1,  3)  and 
decreasing  the  angular  velocity  of  opposite  rotation  motors  (2,  4).  The  rotors  of  quadrotor 
are  located  six  inches  from  the  end  point  of  the  aluminum  rods  and  /  is  the  length 
between  the  rotational  axis  of  each  rotor  and  the  center  of  gravity  of  the  quadrotor.  So  we 
can  assign  the  following  control  inputs,  Ui : 


U,= 


Fj  +  Ft  +  Fj  +  Fj 


m 


U  2  =  FT2  ~  FT4 

u3=ft  -  ft 


U 4  =  (Fj,  +Fj  -  Fj,  -  Fj,  )d 


(85) 


where  F1,i  and  d  are,  respectively,  the  normalized  thrust  force  from  each  ( ith  )  rotor  and 

d  is  the  force  to  moment  scaling  factor  coefficient  depending  on  the  blade’s  Reynolds 
Number,  Mach  number  and  angle  of  attack. 


2.  Waypoint  Navigation 

Waypoint  navigation  for  the  quadrotor  will  take  place  in  two  different  ways.  The 
Qball-X4  quadrotor  will  follow  the  position  of  the  Qbot,  which  acts  as  the  leader. 

a.  Qbot’s  waypoint  navigation  via  the  Qbot  model,  an  autonomous 
waypoint  navigation  program  that  moves  Qbot  through  a  series  of  predetermined 
waypoints  chosen  by  the  user. 

b.  The  quadrotor  navigates  by  following  the  waypoints  taken  from 
the  Qbot  through  the  stream  client  of  the  real-time  workshop.  So,  Qbot’s  Tx,  Ty 
coordinates,  are  taken  as  inputs  so  a  waypoint  state  machine  can  be  built  with  the 
following  states: 

•  Initialize 

•  Takeoff 

•  Follow  waypoint 

•  Land 
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The  waypoint  state  machine  will  allow  quadrotor  to  follow  the 
Qbot’s  position  through  its  controllers.  Alternatively,  a  trajectory  generator  with  inverse 
dynamics  can  be  used  to  generate  new  quasi-optimal  trajectories  for  following  the 
waypoints. 


3.  Linearization  of  Qball-X4  Control 

Since  the  drag  forces  are  negligible  and  the  angles  of  the  orientations  are  very 
small  for  hover  flights  [1],  small  angle  approximation  are  invoiced: 


</>< sc  0 . 1  =>  sin(^)  =  0,  cos(^)  =  1 
0  <sc  0. 1  =>  sin(60  =  0,  cos {6)  =  1 


(86) 


So  the  kinematic  Eq.  15,  can  be  simplified  to: 
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If  we  take  the  derivatives  of  the  previous  equations  and  apply  Eq.28  ,  then: 
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If  Coriolis  term  is  ignored,  the  simplified  equations  of  motions  can  be  derived  as: 
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'<j)  =  p 
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Eq.  56  can  be  written  with  the  controls  as  follows: 


X 

Ux  (-  sin  (j)  sin  yr  +  cos  (j)  sin  0  cos  yr ) 

y 

= 

Ux (- Sin  (j) COS  ^  +  COS  ^  sin  #  sin  ^) 

z 

Ux  cos^cos 0-g 

(89) 


(90) 


Hence,  a  simplified  model  can  be  presented  with  the  state  equation,  that  can  be 
used  for  both  controllers,  the  implemented  and  the  proposed  one: 
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where  X  =  [x,  y,  z,  u,v,  w,  q \  6 ,  y/,  p,  q ,  rf 


[x,  y,  z,  x,  y,  z,  (f>,  0,  yr,  (j>,  0,  yrf 


(92) 


and 


U  =  [Ux,U2,Ui,U4f 


(93) 
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4.  LQR  Controllers  /  Channels  Description 

a.  Rotor  (Actuator)  Dynamics 

The  thrust  that  is  generated  by  each  propeller  is  modeled  by  the  first  order 

system  equation:  FT  .  =  K  —  uf  (93) 

s  +  co 

where  uj  is  the  PWM  input  of  the  actuator,  to  is  the  actuator  bandwidth  and  K  is  a  positive 

gain.  Those  parameters  are  already  calculated  through  experimental  studies  in  Quanser 
[51],  and  they  are  given  in  Table  7. 


Parameter 

Explanation 

Value 

Unit 

K 

PWM  input 

of  the  actuator 

120 

N 

CD 

Actuator  Bandwidth 

15 

rad/sec 

Table  7.  Thrust  parameters 


A  state  variable  vt  will  be  used  to  represent  actuator 
dynamics,  which  is  defined  as  follows: 

v=K—u,  (94) 

S  +  O) 

b.  Roll  and  Pitch  Models 

Assuming  that  rotations  about  the  x  and  y  axes  are  decoupled,  two 
propellers  contribute  to  roll/pitch  axis  already  modeled  and  discussed  in  the  previous 
chapter.  The  rotation  around  the  center  of  gravity  is  produced  by  the  difference  in  the 
generated  thrusts.  If  we  set  A u  the  same  for  pitch  and  roll,  the  moments  of  inertia  are 

found  to  be  the  same  ^ xx  ^ yy  ^  since  the  quadrotor  already  considered  symmetric,  the 
models  can  be  formulated  as  also  stated  in  [5 1]: 
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(95) 
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If  we  combine  these  last  three  equations  and  the  actuator  dynamics  the 
following  state-space  equations  can  be  derived: 
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(97) 


To  facilitate  the  use  of  an  integrator  in  the  feedback  structure  a  fourth  state 

can  be  added  to  the  state  vector,  which  is  defined  as:  ^  or  $  =  ®  and  augment  this 
state  to  the  state  vector,  the  final  roll  and  pitch  models  will  be: 
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So  the  state  space  model  has  the  same  state  and  input  matrices  A  and  B 
and  so  the  angles  will  be  the  same  and  only  one  computation  should  be  done  for  the 
Gains  in  LQR  problem.  The  LQR  model  for  the  pitch  and  roll  controller  is  shown  in 
Figure  44.  The  gains  Kphi,Kpii  are  the  outputs  of  the  Roll  -  Pitch  LQR  Controller  m-file 

shown  in  the  Appendix  A. 
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PITCH  MODEL 
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Figure  43.  Pitch  -  Roll  LQR  controller 
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c. 


Height  Dynamics  Model 


The  motion  of  the  Qball-X4  in  the  vertical  direction  (along  the  Z  axis)  is 
affected  by  all  the  four  propellers.  Assuming  that  the  same  force  thus  PWM  input  is  given 
for  every  propeller  so  the  dynamic  model  of  the  Qball-X4  in  this  case  can  be  written  as: 

..  4  f 

Z  =  — -cos^cos<9  -  g  where  F  =  Kv  (100) 

m 


As  expressed  in  this  equation,  if  the  roll  and  pitch  angular  rates  are 
nonzero  the  overall  thrust  vector  will  not  be  perpendicular  to  the  ground.  Assuming  that 
roll  and  pitch  angles  are  close  to  zero,  using  again  the  Eq  (94)  for  the  state  variable  v  and 
a  fourth  state  for  the  use  of  an  integrator  in  the  feedback  structure,  the  dynamic  equations 
can  be  linearized  to  the  following  state  space  form: 
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The  LQR  model  for  the  Height  controller  is  shown  in  Figure  44.  The  gains 
Khi  are  the  output  of  the  Height  LQR  Controller  m-file  shown  in  the  Appendix  A. 


Height  Model  Gain 


Figure  44.  Height  LQR  controller 
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d.  X-  Y  Position  Model 

The  motions  of  the  Qball-X4  along  the  X  and  Y  axes  are  caused  by  the 
total  thrust  and  by  changes  of  the  roll/pitch  angles.  Assuming  that  the  yaw  angle  is  zero, 
the  dynamics  of  motion  in  X  and  Y  axes  can  be  written  as: 

4  F 

x  = - (cos  (f)  sin  y  +  cos  (j)  sin  0  cos  y/) 

m  (102) 


y  = - (-sin^cos^  +  cos  ^  sin  <9  sin  ^) 

m 


Assuming  that  the  roll  and  pitch  angle  rates  are  close  to  zero,  the 
following  linear  state-space  equations  can  be  derived  for  X  and  Y  positions. 
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(104) 


The  LQR  model  for  the  X-Y  Position  controller  is  shown  in  Figure  46. 
The  gains  KJ,K  vi  are  the  outputs  of  the  X-Y  Position  LQR  Controller  m-file  shown  in 

the  Appendix  A. 
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X  POSITION  MODEL 
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Figure  45.  X-Y  Position  LQR  controller 


e.  Yaw  Model 


As  derived  in  the  previous  chapter  the  motion  in  the  yaw  axis  is  caused  by 
the  difference  between  the  torques  exerted  by  the  two  clockwise  and  the  two 
counterclockwise  rotating  props.  The  motion  in  the  yaw  axis  can  be  modeled  by: 


y/  =  r 


U±  =  ^lAu 
J  J 

zz  zz 


A u  =  w,  +  u2  -  u2  -  u4 


(105) 
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The  yaw  axis  dynamics  can  be  rewritten  in  the  state-space  form  as: 
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(106) 


The  LQR  model  for  the  yaw  controller  is  shown  in  Figure  47.  The  gains 
Kvawi  are  the  output  of  the  Yaw  LQR  Controller  m-file  shown  in  the  Appendix  A. 


Yaw  Model  Gain 


Figure  46.  Yaw  LQR  controller 
f  Motor  Inputs 

The  main  goal  of  the  controller  is  to  accomplish  an  algorithm  of  quadrotor 
control  and  provides  decoupling  of  control  channels  in  steady  state.  Thus  the  control 
inputs  from  the  controller  about  each  axis  v^v^,  v  w  ,  together  with  the  throttle  command 

for  each  rotor  are  combined  to  generate  the  control  inputs  Wj,u2,u3,w4that  are  given  as 
follows: 


This  control  mixing  that  take  place  in  Figure  48  can  be  proved  to 
be  valid  within  the  following  derivations,  showing  that  every  control  achieves  the  desired 
movement  in  each  associated  axis: 


U  |  —  Wj  +  +  u3  +  u4 
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UA=UX  +u3 -u2-u4  =  uth  +v0  +  v¥+uth-v0  +  v¥-(ulh+v^-v¥+uth-v^-v¥)  => 
=>u4=  2v¥ 


Figure  47.  Control  Mixing  Block 
Figure  48. 
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5.  Qbot  Inverse  Kinematics  Procedure 

The  model  of  the  Qbot  described  in  the  previous  chapter  was  described  with  the 
forward  kinematics.  The  controller  that  the  robot  will  use  is  introducing  the  inverse 
kinematics  problem.  The  inverse  kinematics  problem  of  a  mobile  robot  with  initial 
configuration (x,y,<9),=0  is  the  achievement  of  robot’s  target  configuration (x,  y,3)t  .  If 

(x,  y,  i9)/=0  -  (0, 0, 0) ,  then  solving  the  Eq.89,  we  will  have: 


x(t)  = 


y(t)  = 
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LiVn,Jt)+ %<(!)] 

2(Vrig,„(0-Vlefl(t)) 

LlVrighW  +  VlefM 
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^[Vng/,r(0  +  V(Q] 

2(^(0 -%(0) 


sin  [^(vright(t)-Vieft(t))\ 
cos  [j(vright(t)-vleft(t))\  + 


L\.Vr,ShXt)  +  VlefXt)'\ 

bright 


(107) 


For  a  known  time  t  and  a  target  position  (x,  y),  the  last  Eq.  107  will  be  solved  for 
v right >v left  •  Eqs.  83  and  84  provide  a  simpler  strategy  to  drive  a  robot  to  a  target  position 

(x,  y,  9)t  where  the  robot  can  be  rotated  in  place  until  it  will  be  aimed  toward  (x,  v) ,  then 
driven  forward  until  it  will  be  at  (x,  y) ,  and  finally  rotated  in  place  until  the  required 
target  orientation  &  is  met.  From  the  first  two  equations  of  Eq.  80  we  obtain 

■*’w=2>/F77 


v  +1 

y  right  y  left 


Also,  form  the  same  two  equations  we  obtain  &  =  arc  tan 


T| 

vxJ 


(108) 

(109) 


Differentiating  Eq.  1 1 0  yields  &  =  ^ (110) 

x  +y 

V .  -  V 

From  the  other  hand,  the  third  equation  of  Eq.80  reads  i9  =  — —  (111) 

Hence,  F  (112) 

x  +y 

Eq.  108  and  1 12  resolved  for  Vright  and  Vleft  yield  the  controls  required  to  follow 
a  predetermined  trajectory  y(x). 
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C.  DIRECT  METHOD  BASED  CONTROLLERS 

1.  Introduction 

The  classical  indirect  methods  cannot  handle  complicated  problems  in  real  time. 
There  is  a  need  for  different  techniques  that  simplify  the  problem  or  use  numerical 
algorithms  to  provide  near-optimal  rather  than  optimal  solutions  in  real  time.  The 
proposed  controller  introduces  one  of  the  direct  methods  already  worked  in  real  time.  It  is 
quite  easy  to  program  and  provides  effective  optimization  with  feasible  solutions.  Taking 
Prof.  Taranenko’s  ideas  in  early  60s  as  motivation,  the  computer’s  rapid  development 
helped  several  engineers  develop  algorithms  for  real-time  on-board  calculation  of  quasi- 
optimal  trajectories  ([41  ]-[43])  for  combat  vehicles  and  missiles.  There  were  successful 
tests  within  the  Pilot’s  Associate  program  onboard  5th-generation  aircraft  [44],  This 
direct  method  can  be  used  for  unmanned  air  vehicles  (UAVs)  to  generate  approach 
trajectories. 

The  direct  method’s  simplicity  gives  one  the  opportunity  to  develop  the  theory, 
and  eventually  test  a  real-time  trajectory  optimization  scheme  for  the  quadrotor.  In  the 
following  sections  the  general  architecture  of  the  proposed  controller  will  be  developed, 
consisting  of  the  trajectory  generator  and  the  trajectory  follower.  Using  the  parameters 
and  dynamics  of  the  already  working  controller  we  will  introduce  the  trajectory  generator 
which  will  generate  optimal  or  quasi  optimal  feasible  trajectories  so  that  follow  the 
waypoint  pattern  of  the  ground  robot.  This  real  time  capability  provides  regeneration  of 
each  trajectory  during  the  mission  through  feedback  from  the  sensors.  It  allows  for 
changing  the  objectives  for  any  disturbances.  To  accomplish  this  task,  a  reference 
trajectory  and  the  controls  have  to  be  found,  so  that  even  with  LQR  controller,  the 
following  of  the  path  will  be  also  accomplished.  Also,  an  interpolator  must  be  used  to 
provide  samples  of  the  reference  trajectory  at  the  desired  (high  frequency)  rate.  The 
proposed  scheme  is  shown  in  Figure  49.  This  control  scheme  differs  from  the  previous 
one  in  the  fact  that  in  order  to  achieve  a  solution,  the  trajectory  generator  and  the 
controller  both  need  to  follow  the  reference  trajectory. 
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Figure  49.  General  Architecture  of  Joint  Quadrotor  -  Robot  Controller 
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2.  Formulation  of  the  LQR  Problem 

The  objective  is  to  find  a  controller  that  provides  the  best  possible  performance 
with  respect  to  the  given  criterion.  The  requirements  of  the  controller  are: 

•  Optimality 

•  Stability  of  the  closed  loop  system 

•  Desirable  gain  and  phase  margin 

•  Robustness  with  respect  to  unmodelled  dynamics  of  the  plant  and 
environmental  uncertainties 

For  a  linear  state-space  model  of  the  system  dynamics: 

X{t)  =  AX(t)  +  BU  (t)  (113) 

With  an  assumption  of  availability  of  measurements  of  full  state  the  matrix  gain 
Kc  of  the  optimal  control  input  must  be  determined: 

U(t)  =  -KcX(t )  (114) 

So  as  to  minimize  the  performance  criterion  or  the  cost  function 

oo 

J  =  j(XTQX  +  UTRU)dt  (115) 

0 


where  Q  and  R,  are  positive  definite  Hermitean  or  real  symmetric  weight  matrices.  In  fact 
Q  must  be  positive  semi  definite. 

Bryson’s  Rule  defines  an  initial  choice  of  matrices  Q  and  R 


Qu  = 


l 


R:  = 


max  acceptable  _  value  _of  _  X~ 

_ 1 _ 

max  acceptable _  value _of  _  U2 


,i'e{l,2...,i 

Je{l,2..„ 


(116) 


which  corresponds  to  the  following  criteria 

^  n  n 

0  1  1 


(117) 
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For  a  time  dependent  reference  trajectory  Xref(t) ,  the  LQR  control  can  be  applied 

as  a  trajectory  follower  to  minimize  small  errors  between  the  measured  state  x  and  the 
reference  state  Xref  ,  and  the  control  algorithm  will  become: 

u{t)  =  Uref  (0  -  Kc  (X(0  -  Xref  (0)  (118) 

We  have  to  compute  the  gain  matrix  Kc  using  a  linearized  quadrotor’s  model  and 
then  apply  this  control  the  non-linear  model. 


3.  Stability  Analysis 

Since  the  linearization  of  the  quadrotor  plant  is  already  taken  place  at  hover 
condition,  the  control  gains  Kc  were  designed  with  the  weighting  matrices  as  follows: 

2  =  10  5/12x12  and  R  =  diag(\0  5,108,108,108) , 

if  it  is  sure  that  the  actuator  constraints  are  maintained  as  stated  in  [62],  However, 
following  the  generated  trajectory  the  hover  condition  will  be  sometimes  violated.  That’s 
why  a  specific  envelope  where  the  operation  will  be  stable  has  to  be  detennined.  As 
already  derived  in  [63]  a  linearized  stability  set  representing  a  circumference  of  a  circle 
with  a  radius  of  48  degrees  can  be  practical  enough: 

s(t)  =  {0,<f>:02  +<j>2  <r2,r  =  48°} 

So  if  an  extra  constraint  added  to  the  optimization,  that  maintains  angles  (j)  and  0 
within  this  stability  set  within  the  trajectory  generator  the  linearized  time-invariant 
stability  will  be  assured. 


Now  that  we  set  up  the  problem,  we  can  see  the  whole  flow  procedure  within  a 
diagram,  in  order  to  visualize  better  the  technique  to  be  used.  The  direct  method 
optimization  flow  procedure  is  presented  in  the  following  chart,  where  each  component 
shown  will  be  described  in  the  following  sections  of  the  chapter: 
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Figure  50.  Direct  Method  Optimization  Flow  Procedure 
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4.  Reference  Trajectory 

To  approximate  the  Cartesian  coordinates  of  a  vehicle,  its  speed  and  its 
acceleration  (six  states),  we  can  use  functions  that  will  define  the  variety  of  accessible 
trajectories  and  their  choice  will  depend  on  the  particular  problem.  In  general,  the  more 
tenns  those  functions  have,  the  more  accurate  (closer  to  the  really  optimal)  solution  can 
be  found.  Once  the  Cartesian  coordinates,  speed  and  acceleration  are  defined  using 
reference  functions,  the  remaining  states  and  controls  are  detennined  using  inverse 
dynamics  of  original  non-linear  equations  driving  the  system’s  dynamics.  By  using  the 
direct  method  as  Taranenko  suggested  in  [63],  a  big  advantage  over  the  indirect  method 
sis  obtained  since  we  eliminate  the  issue  of  solving  the  Cauchy  problem  for  detennining 
the  trajectory  with  the  given  initial  states  and  control  time-histories,  but  instead, 
introducing  the  desired  trajectory  from  the  beginning,  time-histories  for  all  the  controls 
are  retrieved  through  the  use  of  the  inverse  dynamics. 

One  modification  of  Taranenko ’s  method  is  to  employ  elementary  polynomials 
[44],  [45],  [62]-[64]  as  the  reference  functions.  There  are  other  alternatives,  like 
Chebyshev  polynomials  [65],  [66]  or  Laguerre  polynomials  [67]  and  others.  In  order  to 
compute  the  coefficients,  let  us  consider  as  reference  function,  algebraic  polynomials  of 
degree  “n”  for  x,  y,  z  coordinates  and  use  as  argument  the  virtual  arc  “x”,  given  as 
follows  (the  exact  same  procedure  will  take  place  for  the  other  coordinates,  y  and  z): 


k= 0 


'(0=Z< 


(max(l,£-2))!r* 

T\ 

(max(l,&  -2))!r 


k-i 


*=i  (*- 1)! 

:/(T)  = 


k=2 


'(r)  =  YJ(k-2)aikT 


k—3 


k= 3 


(119) 


The  degree  “n”  of  these  polynomials  is  chosen  from  the  boundary  conditions, 
where  they  have  to  be  specified  accordingly  so  that  all  the  coefficients  aik  will  be 
determined  algebraically.  The  higher  the  maximum  degree  of  the  time  derivative  of  a 
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vehicle  coordinate  at  initial  and  end  points,  the  higher  the  degree  of  the  polynomial.  The 
minimum  degree  of  the  polynomial  to  be  chosen  will  be  given  by  the  equation: 

n  =  d0+df+ 1  (120) 

where  d0,df  are  the  maximum  orders  of  the  time  derivative  of  the  coordinates  at  the 

initial  and  end  points,  respectively.  So  that  at  the  boundary  values  for  the  quadrotor 
coordinates,  the  first  and  second  time  derivatives  at  both  ends  of  the  trajectory  are 
satisfied,  fifth-order  polynomials  should  be  chosen  since  d0  =  df  =  2  .  Applying  Eq.  (85) 

we  define  eighteen  unknown  coefficients. 

Generally,  the  final  part  of  the  trajectories  needs  to  be  smoother,  since  the  control 
has  to  be  more  accurate  while  at  landing  or  at  rendezvous  point.  That’s  why 
d  f  =  3,  x .  =  0 (/  =  1,2,3)  is  usually  proposed. 

In  case  of  df)  =  2,  df  =  3 ,  thus  n  =  6,  where  an  additional  optimization  parameter 
is  applied,  then  24  coefficients  aik  are  obtained.  Subsequently,  if  we  add  two  parameters, 
the  coefficients  will  be  36. 

Expressing  the  six  coefficients  axk,k  =  0,1,. ..5  (the  same  manner  for  aYk  and  a/k ) 
as  a  linear  matrix  equation,  we  will  have  [64]: 
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The  reference  functions  provide  us  with  the  flexibility  to  increase  the  order  of 
approximation  and  derivatives  at  both  ends  using  them  as  additional  varied  parameters.  In 
the  previous  case,  the  only  varied  parameter  is  r/  (since  all  coefficients  are  determined 

from  the  boundary  conditions),  shown  in  Figure  54.  The  diagrams  are  produced  very 
easily  if  you  calculate  the  equation  121  in  Matlab. 


Figure  5 1 .  Variation  of  the  parameter  of  the  reference  functions,  z,  =  var 


If  7th-order  polynomials  are  used,  the  initial  and  final  state,  first  and  second 
derivatives  were  used  as  constraints  attempted  to  be  satisfied  and  the  third  derivatives  at 
both  ends  of  the  trajectory  became  free  variables  along  with  the  virtual  arc  length  [64]: 

x'(t)  =  Px"i(T)  =  an  +  ai4T  +  +  ai6r'  +  an^ 


1  .2,1  3  1 


1 
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x'i(z)  =  PxH(t)  =  an  +ai2z  +  ^ai3z2  +^ai4z3 +-^ai5z4 +  ^ai6z5  +^anrb 
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1  Xl  10  11  2  6  24  60  120  210 
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The  coefficients  will  be  computed  by  solving  the  following  system  of  linear 
algebraic  equations  [64]: 
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(123) 


If  these  equations  are  resolved  for  the  coefficients  aik  in  Matlab  will  yield  [64]: 
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and  the  final  arc  z  f  becomes  the  first  optimization  and  varied  parameter. 
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5. 


Time  and  Space  Decoupling 


Since  the  speed  is  related  to  the  Cartesian  coordinates  by  the  equation  [71]: 

V(t)  =  jx(t)2  +  Y(t)2+Z(t)2 

we  also  specify  a  speed  profde  along  the  trajectory  by  using  time  t,  as  an 
argument,  ending  to  define  the  trajectory  itself,  too. 

In  order  to  decouple  the  trajectory  from  the  speed  profile  we  can  utilize  the 
abstract  argument  r  which  connects  to  time  through  the  variable  speed  factor 

(125) 

at 

By  this  way,  we  manage  to  vary  the  speed  profile  along  the  same  trajectory  by 
changing  the  speed  factor /l (r)  [71]: 

V(t)  =  A(t)^X\t)2+Y\t)2  +  Z\t)2  n26. 


For  the  quadrotor’s  case  though,  it  is  more  useful  to  approximate  the  speed  factor 
with  the  same  procedure  as  for  the  reference  trajectory  before  in  order  to  achieve 
parameterizing  the  speed  factor  so  as  afterwards  compute  the  speed  profile. 

So,  if  we  assume  that: 
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(127) 
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And  for  n=5: 


A(r)  —  .P) (r)  —  ^ \cifr1'  —  Uq+u^t  +  q2t  +u^t  (128) 


k= o 


where  coefficients  are  the  solutions  of: 
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Then,  finally,  the  speed  profile  will  be  computed  as: 

v(t) = pA  (t)^p;(t))2 +{p;xt))2 +{r(t))2 


(130) 
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6.  Qball-X4  Inverse  Dynamics 


a.  Differential  Flatness 

Suppose  the  dynamics  of  a  system  are  described  by  a  set  of  ordinary  non¬ 
linear  differential  equations: 

x  =  f(x,u )  (131) 

where  x(t )  e  X  c  R"  is  the  state  vector  and  u{t )  e U  cl"  is  the  control  vector,  and  f  is 
some  vector  function. 

By  definition  from  [68],  “the  differential  flatness  is  the  expression  of  the 
state  and  control  vectors  in  terms  of  some  output  vector,  y.”  (Without  loss  of  generality 
we  assume  y  to  be  a  subset  of  x,  i.e.  y  =  Cx ,  where  C  is  a  k-by-n  matrix,  k  <  n).  Also 
from  [69],  “For  a  system  to  be  differentially  flat  and  therefore  possess  a  flat  output  it 
requires  a  set  of  variables  y{t)  —  Cx  e  Y  a  RA  such  that: 

•  The  components  of  y  are  not  differentially  related  over  A  ; 

•  Every  state  and  control  may  be  expressed  as  a  function  of  the 
output  vector  y  and  a  finite  number  of  their  time  derivatives,  i.e. 

x  =  hl(y,y,y,y,..)  and  u  =  h2(y,y,y,y,...) 

which  is  essentially  the  inversion  of  the  original  system  x  =  /(x,  u )  with  respect  to  the 

output  vector  y  =  Cx  .” 


b.  Quadrotor ’s  Inverse  Dynamics 

Attempting  to  address  the  differential  flatness  property  in  quadrotor’s 
dynamics,  the  output  vector  will  consist  by  four  components,  since  we  have  four  controls. 
These  components  will  be  the  translational  positions  x,  y,  z,  and  the  yaw  angles,  as  it 
can  be  dynamically  decoupled  from  the  other  states  [70]  (in  case  the  control  input  U  is 
used  to  set  the  yaw  angle  to  zero). 


So,  the  output  vector  Y  can  be  defined  as: 


Y  =  l> 
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Hr  = 


1 3x3  ^3x5  0 


0  0lx5  1  0 
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X  =  cx 


(132) 
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Now,  the  control  inputs  can  be  expressed  as  a  function  of  the  states  and  their 
derivatives  as  follows: 


Ux  =^x  +y2  +{g  +  zf 

u2  =  '<i 

U3=9 

U,=Y 


(133) 


We  have  to  take  the  second  derivative  of  the  states  </>,6  ,  the  rotational  part  of  the 
state  vector,  in  order  to  express  all  the  control  inputs  toward  output  vector.  So,  in  order  to 
express  those  states  in  terms  of  the  output  vector,  we  follow  the  procedure  below: 

If  we  take  individually  the  three  equations  of  Eq.  90  and  by  rearranging  and 
multiplying  the  third  one  with  “tanO”  we  have: 

x  =  Ul  sin  (j>  sin  yr  +  Ux  cos  (j)  sin  #  cos  yr 
y  =  -t/j  sin  ^  cos  yr  +  Ux  cos  (j)  sin  #  sin  yr 
(z  +  g)  tan#  =  Ul  cos ^ sin 9 

If  we  substitute  the  third  equation  to  the  other  two,  we  get: 

x  =  Ux  sin  (f>  sin  y/  +  (g  +  z)  tan  9  cos  y/ 
y  =  -Ul  sin  (j>  cos  y/  +  (g  +  z)  tan  9  cos  yr 

xcosy/  =  U,  sin  (j)  sin  yr  cos  yr  +  (g  +  z)  tan#  cos2  y/  j  (+) 

=>  ,  \=> 

y sin^-  =  — C/j  sin ^ sin ^ cos ^  +  (g  +  z) tan# sin'  y/ J 


I  xcos(^ 


'  xsin(y 


=>  x  cos  y/  +  y  sin  y/  =  (g  +  z)  tan  #  cos2  y/  +  (g  +  z)  tan  #  sin2  y/ 
=^>  x  cos  y/  +  y  sin  y/  =  (g  +  z)  tan  #  iyo^jy^rt^cyr) 


=^>  tan  #  = 


x  cos  y/  +  y  sin  yr 
g  +  z 


=^>  #  =  arctan 


xcos^  +  ysin^ 

g  +  z 


(134) 
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With  the  same  procedure  if  we  multiply  with  the  opposite  way  and  subtract  the 
two  equations  we  get: 

x  =  Ul  sin  <j)  sin  y/  +  (g  +  z)  tan  0  cos  y/ 
y  =  -17,  sin  (f>  cos  y/  +  (g  +  z)  tan  0  cos  y/ 

xsin^/- =  17,  sin 0 sin2  ^  + (g  +  z) tan# sin ^ cos ^  |  7) 

=^>  2  f=> 

y  cos  y/  =  -17,  sin  (f>  cos'  y/  +  (g  +  z)  tan  0  sin  y/  cos  y/  J 


I  xsin^ 


'  xcos^ 


=^>  x  sin  y/  -  y  cos  y/  =17,  sin  (f>  sin2  yr  + 17,  sin  (f>  cos2  y/ 

=^>  xsin^-jicos^  =  17,  sin  (f)  (yoyyjy^+^xcy^) 

.  ,  xshw-  vcosy/ 

=>  sin  (h  = - - — - - — 


=>  (j)  =  arcsin 


x  sin  y/  -  y  cos  y/ 
\jx2  +  y2  +  (g +z)2 


(135) 


Having  the  state  vector  components  expressed  in  terms  of  the  output  vector,  we 
proceed  to  do  so  for  their  derivatives  also  are: 


and 


0  = 


(x  cos  y/  +  y  sin  y/)(g  +  z)  -  (x  cos y/  +  y  sin y/)z 


(g  +  z)"  +  (xcosy/r  +  ysin^/')" 


•  (xcos^  +  'v  sin  ^)(g  +  z)- (g  +  z)  (tan  #z) 

0  — - 2 - 2 - 

(g  +  z)'  +  (xcos^  +  vsin^)" 


0  = 


(x  cosy/  +  y  sin  y/  -  tan  0'z)  {g  +  z) 
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(136) 
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(f>  = 

i>  = 


(3c  sin  y/  -y  cos  y/)Ux  -  ( x  sin  y/  -  y  cos  y/)Ux 


Ux  ^U2  -  (x  sin  y/-y  cos  v\ 
(x  sin  y/-y  cos  y/)Ux  -  Ux  sin  <j)Ul 
U i  -\J U 2  -(xsin^-ycos^)2 
(3c  sin  if r  -  y  cos  y/)  -  Ux  sin  (f> 


<t>  = 


U2  -(xsin^--  vcos^)" 


(137) 


The  second  order  derivatives: 


fj=(g  +  z) 


(x  cos  y/  +  "y  sin  y/  -  tan  ff'z)  -  20  [z  +  (3c  cos  y/  +  y  sin  y /)  tan  61]  ^ 


( g  +  zf  +  (xcos^  +  vsin^1)" 


(x'sin^-'ji'cos^)-^  sin^  (j)  Ux -sin^(x  sin^  - y  cos^)  (J] 

0= - /  - 777 - 77“ - 7 - 77 -  (138) 


U2  -  (x  sin  y/  -  ycosy/y 


U2  -(xsin^- ycosy/\ 


Instead  of  using  the  exact  values,  we  could  also  employ  central  difference 
approximations 


0, 


-20,-6U 

At2 


■i  _  <t>,  -ty-h-i 
9i  At2 


(139) 


(forward  and  backward  approximations  would  be  used  then  for  the  first  and  last 

points). 

Finally,  we  computed  the  states  <j>,B  in  terms  of  the  output  vector  components, 
since  U]  is  already  expressed  in  terms  of  x,  y,  z.  When  the  vehicle  is  in  free  fall, 
singularities  may  occur,  since  g  =  —z .  One  way  to  avoid  it  is  by  constraining  the  input 
such  that  U]  >  1  and  the  pitch  and  roll  such  that  6  <  90°  and  (j)  <  90°  as  stated  in  [48]. 

The  differential  property  of  the  system  provides  us  with  the  opportunity  to  transfer  the 

optimization  from  the  control  space  to  the  Output  space. 
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7. 


Discretization 


As  in  any  numerical  method  we  will  compute  parameters  of  the  systems  in  a  finite 
number  of  points  N  placed  equidistantly  along  the  virtual  arc  z  f  (varied  parameter),  thus 

by  dividing  the  virtual  arc  rf  into  N- 1  equal  pieces  as  shown  in  Figure  so  as  [71]: 


Ar  =  zf{N-\) 
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Figure  52.  Excluding  Time  and  Converting  Back  to  Time.  After  [71] 


We  then  have  N  equidistant  nodes  j  =  1 N.  All  states  and  controls  at  the  first 
point  j  =  1  (corresponding  to^  =r0  =  0)  are  defined.  For  each  of  the  subsequent  N—  1 

nodes  j  =  2,..,  N,  the  corresponding  instants  of  time,  however,  need  to  be  computed  as 
follows  [71]: 


r .  =  (y-l)Ar  =>  Ar  = 


7-1 


A  = 


2Ar 


Aj-i  +  Aj 


ij  -  tj_x  +  At;._j 


(141) 


(142) 

(143) 


So,  the  mapping  between  the  r  and  t  domains  are  defined  by  the  speed  factor 
profile/]; .  We  can  now  proceed  with  computing  the  states  and  controls  in  all  nodes.  We 
compute  the  current  value  of  three  Cartesian  coordinates  and  speed  using  the 
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corresponding  polynomials:  X }  =  Px ( rj ),  Y.  =  PY ( v. )  and  Zj=Pz(Tj)  .  Then,  using  the  Eq. 
141  we  compute  the  time  passed  since  the  last  sample: 


At 


J~l  ST/  T/  \ 


Vj-Vj-J 


(144) 


The  current  time  then  from  Eq.143  equals  to  tj  =  tj  ]  +  Atjt  (t]  =  t0  =  0) ,  and 
therefore  the  current  value  of  the  speed  factor 

At 


X  = 

1  At 


(145) 


i- 1 


Next,  we  convert  r  derivatives  to  time  derivatives  using  chain  rule  relations: 


x  =  —  =  ——  =  xA  and  x  =  ^  'x  +  x  X)A  ,  so  that  we  finally  have: 

dt  dz  dt  dt 


Xj  =  ZjiXjXj  +  A/j) 

Xj  =  Aj[(Xf  +  XjXj)xj  +  AjQXjx)  +  AjXmj)\ 

yj  =  AJyj 

yj=Xj(XJy'j+Xjy'j) 

y)  =  Xj[(Xj2  +  X/'^y)  +  AjQXjy]  +  Ajy])\ 


AjiXjZj+A/j) 

A^+XjX^Zj+AjQX/j+Ajz])] 

¥i,J=Xj{XJy/'J+Xyj) 

To  convert  the  initial  conditions  of  the  states  from  time  derivatives  to  virtual  arc 
derivatives,  we  have  to  use  the  inverse  of  the  above  relations,  so  as  to  compute  the 
controls  and  the  remaining  states. 
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8.  Trajectory  Optimization 

We  have  already  chosen  the  reference  functions  for  the  X,  Y,  Z,  T  and  Zand 
computed  their  coefficients,  introduced  the  inverse  dynamics  to  the  system  and  set  the 
boundary  conditions.  The  next  step  is  to  proceed  with  the  trajectory  optimization  through 
a  specific  optimization  routine. 

a.  Problem  Formulation  in  the  Control  Space 

Normally,  in  order  to  determine  the  optimal  trajectory,  the  optimization 
procedure  take  place  within  the  control  space  regarding  the  applied  constraints  like  state 
constraints,  actuator  (control)  constraints  and  obstacle  avoidance  constraints  within  the 
output  space  and  the  state  space.  The  problem  can  be  set  up  as  [48]: 

min  ®  for  t  e  f  0,  t ,  1  such  that 

£/(0eUciR4  L  3  J 

Y-Cg(X,U)  =  0 

Y0-Cg(X0,U0)  =  0 

Yf  —Cg(Xf,Uf)  =  0 

C(X,U)  <  0  (145) 

where  O ( Y ,  II )  i s  the  cost  function,  the  initial  and  final  constraints  on  the  states  are  set 
according  to  Eq.132  and  Eq.88  at  /  =  Oand  t  =  tf ,  respectfully,  the  dynamic  inequality 

constraints  on  the  trajectory  (for  obstacle  avoidance)  and  on  the  states  and  inputs  (to 
avoid  singularities  and  to  provide  constraints  on  the  control  signals)  are  expressed 
through  the  set  of  functions  C(X,U). 

b.  Cost  Function 

The  cost  function,  O ,  is  a  quantitative  measure  of  the  optimality  of  the 
trajectory  and  consists  by  the  sum  of  two  components,  the  running  costs  and  the  terminal 
cost.  If  the  running  costs  (battery  consumption)  are  proportional  to  the  average  velocity, 
the  objective  function  can  be  defined  as  [48]: 
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(146) 


O  =  (1  -  w) -  J  yjPxx2  -t-T-Jv2  +  Pxz2dt  +  w(tf  -Tf 

t  0 

where  w,  f]  P2  P,  are  the  weighting  factors  (not  necessarily  equal  to  each  other),  and  T  is 
the  predetennined  time  of  arrival.  The  minimum-time  case  takes  place  when  w=l  and 
T=0  and  the  minimum-fuel  case  when  w=0  and  Pl-P2-Pi.  So,  it  is  clear  that  the 

mission  scenario  will  determine  what  the  cost  function  is  and  how  many  weighting 
factors  have  to  be  adjusted. 

Having  proved  through  differential  flatness  that  the  state  vector  x  and  the 
control  vector  u  can  be  both  expressed  via  the  derivatives  of  the  output  vector  y 

X  =  hl{Y),U  =  h2(Y)  (14y) 

We  can  reformulate  the  optimization  problem  in  the  output  space. 


c.  Problem  Formulation  in  the  Output  Space 

If  optimization  takes  place  within  the  output  space  (differential  flatness 
properties)  as  opposed  to  the  control  space,  it  would  be  very  useful  because  the 
constraints  occurring  for  example  from  obstacle  avoidance  happens  in  the  output  space, 
hence  the  computation  time  for  constraint  handling  is  reduced  drastically.  As  opposed  to 
(145)  the  problem  can  now  be  refonnulated  as  follows: 

min  ®  for  t  e  |~0,  tf  1  such  that: 

[/(OeUc'Ji4  L  1  -I 


r0-g*(r(0))  =  o 

Yf-g\tf)  =  0 
C*(Y)  <  0 


(148) 


where  =  Cg(X’U )  =  Cg{hl{Y),h2{Y))  C\Y )  =  C(X,U)  =  C(th(Y),h2(Y))  come 

from  Eq.  147.  With  proper  parameterization  this  problem  can  be  solved  in  MATLAB 
using  the  optimization  toolbox  function  fmincon. 


106 


d.  Parameterization 

In  order  to  reduce  the  dimension  of  the  problem  to  a  finite  amount,  it  is 
suggested  that  the  three  translational  outputs  ( x ,  y  and  z)  be  parameterized  (the  fourth 
output,  the  yaw  angles ,  is  assumed  to  be  zero). 

Thus  the  equations  derived  so  far  will  become: 


6  =  arctan 


x 

g  +  z 


(149) 


(j)  =  arcsin 


y 


<]x~+y  +(g+zY 


(150) 


The  derivatives  also  are: 


6=x(g+2)-m  or 


(g  +  z)2  +  x2 


0  = 


(x  -  tan  ffz) (g  +  z) 


(g  +  z)‘  +  x: 


<!> 


-y^x  +y2  +(g  +  zf  +  y(xx+yy  +  z(g  +  z )) 


X  +ji“+(g  +  z)  ^ Jx  +(g  +  z) 


or 


(151) 


0  = 


-yU\  +  yU\  _-y-u^m(j) 

U0U?-f 


(152) 


The  second  order  derivatives: 


8  =  (g  +  i) 


(x  -  tan  ff'z)  -  26l[z  +  (x  +  y)  tan  6*]  ^ 


(g  +  z)~  +  x2 


(153) 


*  = 


[(-yVf/jSin^]  ^[t/j  -sin^(-y)]f/1 


PM1 


T  T  2  ••  2 

Ux  -y 
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V.  SIMULATION 


A.  QUADROTOR  MAIN  INTERFACE 

The  Quarc  /  Simulink  top  level  realization  of  Quadrotor  model  and  controllers 
developed  are  shown  in  Figure  54.  This  the  main  screen  you  see  when  you  open  the 
quadrotor  model  in  Simulink.  It  was  separated  into  different  components  for  friendly  use 
by  the  operator. 

Qball-X4  Waypoint  controller 

Issue  waypoints  to  the  Qball  for  navigation  control. 

Switch  between  joystick  and  closed-loop  control  using  the  switches  inside 

the  Mode  Control  subsystem.  j 

'  j  Bad  Link 

In  closed-loop  flight,  control  the  position  of  the  Qball-X4  by  setting  height 

and  heading  in  the  Position  Commands  subsystem. 

View  IMU  data  and  motor  output  signals  in  the  HiQ  subsystem. 

Data  is  logged  to  a  host  MAT-file  in  HiQ\SAVE  DATA  (black  box) 


-  - . ■!  .  Pitch  Controller  Roll  Controller  Yaw  Controller  Control  signal  mixing 

HiQ  Joystick  from  host 

Figure  53.  Simulink  Representation  of  Qball-X4  Controller 

The  system  consists  of  the  following  subsystems  blocks: 

1.  Positions  Commands  Subsystem 

The  Position  Commands  subsystem  contains  the  waypoint  state  machine 
described  in  the  previous  section,  where  its  outputs  are  the  throttle  command,  the  x,  y,  z 
position  and  the  heading  commands  as  shown  below  in  the  simulink  diagram.  The  Y  axis 
is  called  Z  inside  the  optitrack  software.  So,  where  Z  is  meant  Y  and  where  height  is 
meant  Z. 
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Gain  Heading 


Heading  command  (active  when  heading_mode  set  to  1) 


heading  command  Goto€ 


0*pi/180 


< 


heading_cmd 


Figure  54.  Positions  Commands  Subsystem 


As  far  as  the  heading  command  is  concerned,  a  stable  heading  set  up  to  8  degrees 
was  used  for  the  experiments.  This  eliminates  the  small  disturbance  of  the  heading  when 
the  quadrotor  takes  off.  I  assume  this  phenomenon  exists  because  of  the  metallic  objects 
inside  the  lab  that  cause  some  interference  in  the  magnetometer  measurements. 

2.  Mode  Control  Subsystem 

The  Mode  Control  Subsystem  allows  the  operator  to  select  height,  position  and 
heading  modes  for  autonomous  or  4-channel  joystick  control  as  shown  in  Figure  56. 


110 


CLOSED-LOOP  ON 


Set  to  1  for  automated  height  control  using  sonar. 
Set  to  0  for  joystick  control  over  throttle. 


height_mode 

Goto3 


Set  to  0  for  joystidr  roll  and  pitch  control. 

Set  to  1  for  autonomous  position  oontrol  using  OptiTrac*. 


position_mode 


GctcS 


Set  to  1  for  autonomous  heading  control  using  magnetometer. 

Set  to  0  for  joystick  yaw  oontrol. 

NOTE:  first  make  sure  the  magnetometer  is  calibrated  and  the  offset  is  set  in  the  field 
HiQ\Calculate  Roll  Pitch  Heading  Height\Calculate  Heading  .camera  frame  heading  offset 

heading_mode 

Goto2 


Figure  55.  Mode  Control  Subsystem 


3.  Calculate  Roll  Pitch  Heading  Height  Subsystem 

This  subsystem  computes  the  vehicle’s  orientation  through  the  calculation  of  roll, 
pitch,  magnetic  heading  and  the  x,  y  magnetometer  components  of  the  quadrotor. 
Complementary  filter  was  used  to  correct  roll  and  pitch  components. 


Calculate  Magnetometer  Calculate  Magnetometer 

X  Component  Z  Component  Calculate  Heading 


Figure  56.  Calculate  Roll  Pitch  Heading  Height  Subsystem 
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4.  HiQ  Subsystem 

The  HiQ  subsystem  consists  of  the  Hardware-  In-the-Loop  (HIL)  blocks  used  to 
configure  the  HiQ  acquisition  card  and  read  or  edit  the  values.  It  contains  the  Motor  / 
PWM  outputs  that  power  the  whole  system  and  initialize  the  sensors  to  get  the 
measurements  through  the  various  selectors.  The  Gain  simulink  block  enables  or  disables 
the  motors  by  multiplying  the  motor  input  signals  by  the  value  of  1  or  0,  respectively. 
Inside  this  subsystem  you  can  also  check  the  battery  voltage. 


Cba*  l>Hq_aero-0) 


Motor  outputs:  bade,  front,  left,  right 


motcr_outputs 


watchdog 


pwm output] 


Initialize  sensors 


-►Em- 

Select  gyros 

-Hu  Yh 


gyroscope  x.  y,  z 


Select  accelerometers 


-Hu  n 


accelerometer  x.  y.  z 


Select  magnetometers 


lagnetometer  x.  y.  z 


n 


Select  battery 


Int  battery  voltage 


battery  voltage 


-*JU  Y  r 


Figure  57.  HiQ  Subsystem 
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5. 


Joystick  from  Host  Subsystem 


This  subsystem  is  an  alternative  power  system  to  the  motors  through  the  4- 
channel  joystick.  It  receives  streaming  data  from  the  host  model.  If  the  IP  address  is  set 
up  properly  for  the  ground  station  in  the  Stream  Client  URI,  then  the  Qball  is  able  to 
connect  to  the  host  model.  It  gives  the  same  commands  as  in  the  Position  Commands 
subsystem  and  receives  the  data  packet  from  the  optitrack  system.  For  safety  reasons,  if 
the  communications  from  the  host  is  interrupted  for  1  consecutive  second,  the  QBall  is 
ordered  to  land. 


Stream  Client2 


"tcpip:// 182. 168. 1 .219: 1 8006' 

Figure  58.  Joystick  from  Host  Subsystem 
6.  Save  Data  Subsystem 

The  “Save  Data”  subsystem  saves  all  the  data  collected  from  every  flight  of  the 
quadrotor  into  a  MAT  file  through  the  Quarc  block  “To  Host  file”  for  further  processing 
or  plotting.  Figure  60  shows  how  all  the  different  signal  outputs  collected  and  saved. 
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Figure  59.  Save  Data  Subsystem 


B.  QBall  X4  WAYPOINT  NAVIGATION 
1.  Waypoints  Input 

The  waypoints  foe  the  vehicle’s  navigation  will  be  introduced  through  the  script 
“Initialize_Qball_Waypoints.nl  showing  in  Appendix  A,  where  the  user  defines  the 
position  of  the  waypoints  in  the  operation  X-Y  area  (again  Y  axis  is  represented  in  the 
model  as  Z  because  of  the  optitrack  system  notation).  The  platform  where  the  waypoints 
are  set  is  shown  below: 


Figure  60.  Waypoints  Input  Diagram 
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2.  Waypoint  State  Machine 

The  waypoints  will  be  followed  through  the  waypoint  state  machine  shown  in 
Figure  62. 


Waypoint  stale  machine 


Figure  61.  Waypoint  State  Machine 

The  height  will  be  held  constant  at  0.6  m  (hgt  coordinate)  through  the  sonar 
controller.  Tx  and  Tz  are  the  coordinates  taken  from  the  previous  section  and  represent 
the  coordinates  of  x-  and  y-axis,  respectively  (This  representation  came  up  from  the 
optitrack  system  set  up).  The  function  controls  the  machine  is  shown  in  Appendix  A. 
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3.  Waypoint  Tracking 

The  trajectory  will  be  followed  through  the  implemented  LQR  controller 
described  in  the  previous  chapter  and  for  specific  weight  factor  matrices  that  will 
improve  the  performance.  The  LQR  gains  are  shown  below: 

Roll  -  Pitch  Controller  Weight  Factors:  Q=  [100  0  22000  10]  -  R=3000 

Output  LQR  Gains:  k(l)  =  -20.047  +  0.000  i 
k(2)  =  -5.618  +  6.128  i 
k(3)  =  -5.618  -6.128  i 
k(4)  =  -0.316  + 0.000  i 

X-Y  Controller  Weight  factors:  Q=  [50  2  100  0.1]  -  R=50 

Output  LQR  Gains:  k(  1)  =  12.080  +  0.000  i 
k(2)  =  -1.762  +  1.604  i 
k(3)  =  1.762  -  1.604  i 
k(4)  =  -0.045  +  0.000  i 

Height  Controller  Weight  factors:  Q  =  diag([l  0  50])  -  R  =  5000000 

Output  LQR  Gains:  k(l)  =  -0.517  +  0.890  i 
k(2)  =  -0.517  -  0.890i 
k(3)  =  -1.024  + 0.000  i 

Yaw  Controller  Weight  factors:  Qy  =  diag([l  0.1])  -  Ry  =  1000 

Output  LQR  Gains:  k(l)  =  -3.762  +  1.287  i 
k(2)  =  -3.762  -1.287  i 

The  representation  of  how  accurate  is  the  waypoint  tracking  is  shown  in  figure  63 
(two  trajectories).  The  quadrotor  takes  off,  stabilizes  in  the  assigned  altitude  and  then 
follows  the  waypoints  1  to  6  added  from  the  operator  (shown  in  Figure  61),  where  the 
starting  point  is  the  zero  point  (0,  0)  of  the  localization  system.  When  each  waypoint  is 
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found,  the  quadrotor  has  to  stay  5  sec.  A  little  overshoot  takes  place  because  of  the 
acceleration,  it  has  already  gained.  The  smaller  the  distance  the  smaller  the  overshoot. 
The  second  trajectory  takes  place  with  low  battery.  Through  the  experiments,  it  was 
shown  that  as  the  battery  goes  off;  the  response  of  the  quadrotor  is  degraded. 


Figure  62.  QBall  Actual  Trajectories  (  With  Full  battery  and  low  battery  ) 
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4. 


Plots 


Several  plots  can  be  obtained  using  the  Save  data  interface.  Some  of  them  can  be 
really  useful  for  understanding  the  response  of  the  vehicle  and  the  controller;  some  others 
just  for  testing  purposes  in  order  to  check  if  we  can  obtain  data  from  the  quadrotor 
sensors.  The  PWM  input  of  every  motor  toward  time  is  shown  Figure  64.  The  initial 
value  is  the  5%  of  the  20  ms,  thus  1  ms  that  is  the  minimum  throttle  and  it  goes  up  to  9% 
of  the  20  ms,  thus  1.8  ms  (maximum  throttle=2  ms). 
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Figure  63.  PWM  Input  for  each  motor 


The  following  figures  depict  the  data  from  each  sensor  and  can  be  used  for  testing 
purposes.  Figure  65  shows  the  gyroscope  measurements  in  rad/s,  where  small  variations 
around  zero  take  place.  Figure  66  shows  the  accelerometer  measurements  in  m/s.  The  Z 
acceleration  oscillates  around  -10,  the  value  of  the  gravitational  acceleration.  Figure  67 
shows  the  magnetometer  measurements  in  Gauss,  with  the  Z  value  to  be  the  largest. 
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Figure  64.  Gyroscopes  measurements 
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Figure  65.  Accelerometers  measurements 
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Figure  66.  Magnetometer  measurements 
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The  observable  and  magnetic  heading  are  shown  in  Figure  68.  The  magnetic 
heading  is  the  straight  calculation  of  the  atan2  (mag_y  /  mag_x),  where  mag_x,  mag_y 
are  the  measurements  of  the  magnetometer.  The  observable  heading  is  the  corrected  one 
using  feedback  from  the  magnetic  heading. 
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Figure  67.  Observable  and  magnetic  heading 


The  following  plots  present  the  battery  voltage  and  the  chosen  modes  for 
autonomous  navigation.  If  the  mode  is  0  the  navigation  is  taking  place  through  the  4 
channel  joystick,  else  if  the  mode  is  1  then  the  navigation  is  autonomous.  The  battery 
voltage  drops  from  flight  to  flight,  if  the  voltage  goes  under  10.6V  then  the  battery 
becomes  useless. 
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Figure  68.  Battery  voltage  and  height-heading-position  mode  (auto=l) 
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The  following  plots  in  figures  show  the  comparison  of  the  measured  and  the 
commanded  X,  Y,  Z  data.  The  small  differences  shown  is  having  to  do  with  the 
disturbances  occurred  during  the  flight  as  well  as  with  the  linear  controller  it  was  used. 
The  difference  in  height  between  measured  and  command  Z  is  because  the  optitrack 
system  captures  the  reflector  placed  in  the  top  of  the  quadrotor,  thus  0.6  m  from  the 
ground. 


-  Y  command 

-  Y  measured 
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Figure  69.  X-  Y  position  comparisons 


-  Z  command 

-  Z  measured 


Figure  70.  Height  position  comparison 


The  comparison  of  the  measured  and  commanded  roll  and  pitch  data  is  shown 
below.  The  difference  actually  in  both  roll  and  pitch  response  is  quite  small.  The 
measured  roll  and  pitch  values  are  already  filtered,  as  it  is  noticed  form  the  graphs.  The 
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only  distinct  difference  is  happening  in  the  first  seconds  when  the  takeoff  takes  place.  It 
is  a  phenomenon  under  investigation  since  the  quadrotor  tends  to  roll  a  little  bit  as  it  takes 
off  and  climbing  to  the  assigned  height.  The  roll  controller  gains  should  be  tested  more  in 
order  to  eliminate  this  effect. 
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Figure  71.  Roll  comparison 


Figure  72.  Pitch  comparison 
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B.  QBOT  WAYPOINT  NAVIGATION 


1.  Waypoints  Input 

The  waypoints  will  be  introduced  through  the  script  “Initialize  Qbot  Waypoints.m 
showing  below  where  the  user  defines  the  position  of  the  waypoints  in  the  operation  area 
(x-y  area). 

As  we  see  in  the  m-  file,  there  is  a  calibration  file  loaded,  coming  from  a  magnetic 
calibration  process  that  took  place  before  the  navigation  process  began,  in  order  to  set  up 
the  compass  to  the  environment  of  the  laboratory  taking  into  account  the  declination  of 
Monterey  that  is  different  from  Toronto  (Quanser  company  location)  where  the  initial 
calibration  was  done.  The  simulink  model  of  the  compass  and  the  model  that  initiates  the 
calibration  are  shown  in  Figures  74  and  75,  respectively. 


Figure  73.  Compass  Model 
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Figure  74.  Qbotmagnetometercalib  model 
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The  Calibrated  magnetometer  x  (red),  y(green)  measurements  are  shown  in  Figure 
76.  The  difference  in  time  occurs  because  of  the  offset  imposed  for  better  representation. 
The  calibration  is  succeeded  when  the  values  come  closer  to  the  absolute  1,  -1.  So  the 
performed  one  is  better  for  the  y  measurements. 
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Figure  75.  Calibrated  Magnetometer  X,  Y  Measurements 

The  platform  where  the  waypoints  are  set  are  shown  in  Figure  77.  Six  waypoints 
are  chosen.  The  Qbot  is  placed  in  the  zero  point  of  the  localization  system. 


Calibrated  Magnetometer  X,  Y  Measurements 


Figure  76.  Waypoints  Input 
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2. 


Waypoint  Tracking 


The  waypoint  tracking  takes  place  through  the  motion  planner  which  uses  the 
inverse  kinematics.  It  takes  as  inputs  initial  and  target  position  of  the  robot,  the  time  step 
and  after  implementing  a  feedback  structure,  computes  the  right  and  left  velocity  and  the 
distance  to  the  target  position. 


Figure  77.  Motion  Planner  Simulink  Representation 
3.  Trajectory  representation 

The  trajectory  for  the  waypoint  tracking  is  shown  in  Figure  79.  The  Qbot  starts 
from  zero  point  and  follows  the  assigned  waypoints.  The  curves  shown  in  each  waypoint 
is  the  rotation  of  the  Qbot  to  the  required  heading  and  they  are  being  shown  because  the 
diagram  tracks  the  velocity,  too.  The  Qbot  computes  the  right  and  left  wheel  velocities 
required  for  following  the  trajectory  from  one  waypoint  to  another  through  the  inverse 
kinematics,  and  with  the  forward  kinematics  moves  toward  each  waypoint.  It  also  takes 
input  from  the  compass  and  provides  feedback.  This  is  the  alter  to  the  initial  trajectory 
between  waypoints  3  and  4. 
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4.  Plots 


The  following  plots  depict  the  actual  x,  y  position  components  and  the  orientation 
theta  of  the  Qbot  as  well  as  the  optitrack  measurements  of  these  components  for  the 
waypoint  pattern  described  earlier.  The  difference  between  the  encoded  data  and  the 
optitrack  data  is  having  to  do  with  the  fact  that  the  optitrack  system  captures  the  motion 
of  the  reflector  placed  on  the  top  of  the  camera  and  not  the  robot  itself.  The  “topt’ 
diagram  is  so  “messy”  because  the  robot  adjusts  its  position  and  orientation  several  times 
till  it  finds  itself  within  the  limits  of  each  waypoint. 
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Figure  79.  Qbot  x,  y,  theta  plots 
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20  40  60 
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In  order  to  find  the  final  orientation  theta  that  the  robot  will  apply  to  go  to  each 
waypoint,  these  components  have  to  be  measured.  “Dist”  is  the  wheel’s  distance  travelled 
in  a  sample  time,  “Ang“  is  the  angle  turned  in  a  sample  of  time,  “heading  abs”  and 
“Omag”  are  the  absolute  and  magnetic  heading  measured  from  the  compass  model  and 
finally  the  Distance  t  is  the  distance  to  the  target  measured. 
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Figure  80.  Qbot  heading  and  distances  plots 


C.  QBot  -  Qball  COOPERATION  WAYPOINT  NAVIGATION 

The  cooperation  of  the  two  vehicles  will  take  place  in  the  form  of  Leader(  Qbot)  - 
Follower  (QBall).  The  Qbot  navigation  will  continue  taking  place  as  before  with  the 
motion  planner.  In  this  model  though,  a  real  time  Data  Streamer  has  to  be  set  in  order  to 
send  the  actual  position  to  the  QBall  and  then  the  quadrotor  starts  his  trajectory  following 
this  positions. 


Display 


Figure  8 1 .  “Stream  to  Qball”  subsystem 
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With  the  above  streamer,  the  waypoint  state  machine  will  have  as  input  the 
coordinates  of  every  waypoint  instead  of  having  defined  by  the  user. 
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Figure  82.  Qball  Waypoint  State  Machine 


The  challenge  is  for  the  host  localization  model  to  recognize  both  models  at  the 
same  time  and  send  the  actual  data  to  the  two  controllers.  That’s  why  a  modification  in 
the  Qball  and  Qbot  host  localization  files  needed  to  be  made  for  the  cooperation.  The 
localization  models  for  each  vehicle  and  the  modification  for  their  cooperation  are  shown 
below: 


}  HOST_qbot_localization  * 

File  Edit  View  Simulation  Format 

Tools  QUARC 

Help 

D  Q  S  ® 

O  j 

T|nf 

J  External 

This  model  runs  on  the  host  ground  station  and  sends 
optitrack  data  to  the  Qbot. 

Run  this  model  first  before  starting  the  Qbot  control  model. 

This  model  can  be  left  running  -  the  Qbot  model  will  reconnect  when  it  is  started. 


Constantl 

Figure  83.  Host  Qbot  Localization 
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:ile  Edit  View  Simulation  Format  Tools  QUARC 

Help 

□  si  y  a  m 

v  P 

(External 

J  SB  i 

3  #  ft 

This  model  runs  on  the  host  ground  station  and  sends 
joystick  and  optitrack  data  to  the  Qball-X4  model. 

Run  this  model  first  before  starting  the  Qball-X4  control  model. 

This  model  can  be  left  running  -  the  Qball-X4  model  will  reconnect  when  it  is  started. 


Display 


Figure  84.  Host  Qball  Localization 


This  model  runs  on  the  host  ground  station  and  sends 
joystick  and  optitrack  data  to  the  Qball-X4  model. 

Run  this  model  first  before  starting  the  Qball-X4  control  model. 

This  model  can  be  left  running  -  the  Qball-X4  model  will  reconnect  when  it  is  started. 


Figure  85.  Host  Qbot  -  Qball  Localization  (Modification) 
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The  “Optitrack  Measurements”subsystem  describes  how  the  localization  through 
the  optitrack  motion  cameras  is  done  for  the  two  models.  The  calibration  file  from  the 
cameras  is  saved  in  the  “Optitrack  Point  Cloud“  Quarc  block. 


f)  HOST Jocai<zation.M>lionii/OptiT rack  Measurements  * 

File  tdit  Vie*  Simulation  Format  Tools  QUARC  Help 

□  &U&  B  <-  ->•  iT  I  £2  1  >  T  R  l&oe" a  3  S  IS  @  ®  m  B  H  IB 


Figure  86.  “Optitrack  Measurements”  Subsystem 


The  both  vehicles  experience  the  same  response  as  operating  alone  since  the 
cooperation  takes  place  with  the  same  controllers  and  waypoint  state  machines/  motion 
planners  with  the  difference  that  the  waypoint  positions  are  sent  from  the  Qbot  and  not 
form  the  operator. 
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VI.  CONCLUSIONS  -  FUTURE  WORK 


A.  CONTRIBUTIONS 

This  thesis  focused  on  the  design  of  a  framework  for  achieving  cooperation 
between  a  quadrotor  unmanned  vehicle,  controlled  by  a  LQR  controller,  and  a  ground 
robot  applying  inverse  kinematics  as  a  control  strategy.  Since  not  much  work  has  been 
done  for  a  controller  using  inverse  dynamics  for  quadrotors  UAVs,  a  direct  method  for 
this  controller  was  also  introduced. 

The  main  contributions  of  this  thesis  are  presented  as  follows: 

•  The  successful  set  up  of  the  laboratory  environment  is  achieved.  This 
consisted  of  the  localization  system  with  ten  motion  capture  cameras,  the 
ground  station,  four  ground  robots  and  four  quadrotors.  For  this  thesis, 
only  one  quadrotor  and  one  ground  robot  was  used. 

•  Applying  Newton’s  laws,  the  six  degree  of  freedom  mathematical  model 
of  a  quadrotor  is  derived,  including  dynamic  analysis  and  design  of  the 
control  inputs. 

•  A  two  dimensional  mathematical  model  for  a  ground  robot  is  also  derived, 
from  the  basic  physics  and  robot  kinematics. 

•  A  LQR  controller  is  designed  for  the  position  and  attitude  control  of  the 
quadrotor,  taking  place  in  different  decoupled  channels  and  under  various 
flight  tests  for  improvement  of  their  performance. 

•  Applying  forward  and  inverse  kinematics,  the  control  of  the  ground  robot 
is  achieved.  The  successful  data  receiving  from  robot’s  various  sensors  is 
also  tested  (Camera,  IR  detectors,  Bumper,  Sonar  detectors). 

•  A  waypoint  scenario  for  the  navigation  of  the  two  vehicles  individually  or 
simultaneously  is  implemented. 

•  A  proposed  controller  utilizing  direct  method  with  inverse  dynamics  for 
real-time  control  of  the  quadrotor  UAV  is  partly  designed  focusing  on 
optimization  over  quasi  optimal  solution. 

The  ground  robot’s  camera  is  placed  and  tested  onboard  the  quadotor  as  an 
additional  sensor,  obtaining  successful  image. 
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B.  CONCLUSIONS 

The  following  conclusions  can  be  drawn  based  on  the  research  of  this  thesis: 

•  Quadrotor  UAV  can  be  linearized  and  controlled  through  linear  control 
methods  quite  successfully  if  the  flight  tests  are  not  far  from  the  hover 
conditions; 

•  The  LQR  controller  works  well  enough  when  the  waypoints  distances  are 
not  very  close  to  each  other  (experiences  higher  overshoot)  and  not  very 
far  away  (the  trajectory  following  becomes  less  precise). 

•  The  LQR  controller  can  be  used  as  a  path  following  controller  together 
with  the  quasi-optimal  trajectory  generator  for  real-time  operations 
achieving  better  performance  toward  any  disturbances  or  for  obstacle 
avoidance. 

•  The  quadrotor  QBall  experiences  limitations  of  operation  because  of  the 
battery’s  low  capacity.  Furthermore,  the  flight  performance  is  degraded  as 
the  battery  capacity  and  life  reduces. 

•  The  ground  robot’s  very  low  speed  is  a  disadvantage  for  the  leader  - 
follower  scenario  of  the  two  vehicles,  forcing  the  operator  to  reduce  the 
quadrotor’ s  velocity  in  order  to  assist  for  the  accomplishment  of  the 
mission. 

•  The  robot’s  Logitech  camera  has  a  slow  rate  of  response,  even  for  the  slow 
velocities  of  the  robot.  It  is  not  suggested  to  be  used  for  faster  vehicles  like 
quadrotors  unless  it  is  supposed  to  operate  on  a  standoff  basis 


C.  FUTURE  WORK 

The  recommendations  for  future  work  are: 

•  Test  to  the  whole  length  the  direct  method  model  and  the  trajectory 
generator  with  LQR  controller  for  different  scenarios. 

•  Achieve  the  cooperation  of  two  or  more  quadrotors  or  two  or  more  ground 
robots  involving  collision  avoidance  scenarios. 

•  Use  multiple  quadrotors  to  observe  an  object  from  different  angles  and  all 
of  them  arrive  simultaneously  to  allow  for  military  reconnaissance 
mission,  a  search  and  rescue  operation,  or  an  industrial  inspection  routine. 

•  Implement  different  controllers  for  path  following  instead  of  LQR. 
Integral  backstepping,  a  combination  of  PID  and  backstepping  was  proved 
the  best  solution  for  controlling  a  OS4  quadrotor  in  EFL’s  research  so  it  is 
a  possible  solution. 
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•  Implement  a  controller  for  the  non-linear  dynamic  model  of  the  quadrotor, 
to  obtain  the  best  performance. 

•  Develop  a  system  for  the  ground  vehicle  that  it  will  use  a  terrain  map 
provided  by  the  quadrotor.  The  quadrotor  can  use  a  added  camera  or 
different  sensors  for  image  processing  in  order  to  construct  a  map  of  the 
terrain  around  the  ground  robot  so  as  to  navigate  to  a  goal  position. 

•  Finally,  since  the  ground  robot  consists  of  a  lot  of  free  positions  for  other 
sensors,  test  which  other  sensors  widely  used  in  experiments  are 
compatible  with  quanser  computer,  gumstix.  This  will  allow  to  use  these 
sensors  in  the  quadrotor  as  well. 
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APPENDIX 


MATLAB  /  SIMULINK  DOCUMENTATION 


setup  qball  x4.M  File 

A  MATLAB  script  that  is  run  whenever  the  Qball  controller  model  is  opened.  This  script 
runs  several  other  scripts  to  initialize  model  and  controller  parameters. 

%  This  file  runs  the  calibration  and  initialization  files  for  the 
Qball. 

%  This  file  is  run  automatically  when  the  model  is  loaded  or  can  be  run 
%  manually. 

%  Complimentary  filter  design  for  roll/pitch  measurements, 
filter  design; 

%  LQR  gains  and  other  controller  parameters  for  the  Qball. 
controller  design; 


filter  design.m  File 

A  script  containing  the  properties  of  the  complementary  filter  used  to  estimate  the  Qball's 
roll  and  pitch. 

t=l  0  ; 

s  =  tf ( ' s '  )  ; 

Gg  =  t A2*s/ (t*s+l) A2 
Gi  =  (2*t*s+l) /  (t*s+l) A2 


controller  design.m  File 

A  script  used  to  compute  the  LQR  controller  gains  used  in  stabilizing  the  Qball's 
orientation  and  position.  This  file  is  run  by  the  setup_qball_x4.m  script. 

LQR  Pitch  and  Roll  Controllers  M-file 

wnom  =  15; 

L  =  0.2; 
w  =  wnom; 

K  =  120; 

J  =  0.03; 

Jyaw  =  0.04; 

CLimit  =  0.025; 

M  =  1.4; 
g  =  9.8; 

Am  =[010 

0  0  2*K*L/J 
0  0  -w] ; 

Bm  =  [0  0  w  ]  '  ; 
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Aobs  =  Am'  ; 

Bobs  =  eye ( 3 ) ; 

Qobs  =  diag([.001  10000  .01]); 

Robs  =  diag ( [  1  1  1  ])*1; 

Kobs  =  lqr (Aobs , Bobs , Qobs ,  Robs ) 

Kobs  =  Kobs ' ; 

Aobs  =  Aobs ' -Kobs*Bobs ' ; 
eig (Aobs ) 

Bobs  =  [Bm  Kobs] 

Cobs  =  eye ( 3 ) 

Dobs  =  [0000 
0  0  0  0 
0  0  0  0  ]  ; 

%  augment  with  integrator 
Ai  =  [Am  [0  0  0  ]  ' 

1000]; 

Bi  =  [Bm '  0  ]  '  ; 

Ci  =  eye  (4); 

Di  =  [0  0  0  0  ]'; 

Q  =  diag ( [100  0  22000  10] ) ; 

R  =  30000; 

ki  =  lqr (Ai, Bi, Q, R) ; 
rp  eig  =  eig (Ai-Bi*ki) ; 

fprintf  ('************************************************  \n  '  )  ; 
fprintf ( 'ROLL,  PITCH  DESIGN  \n ' ) ; 

fprintf (  'P  =  %5.3f  D  =  %5.3f  Actuator  =  %5.3f  I  =  %5.3f  \n\n',ki(l), 
ki  (2)  ,  ki  (3)  ,  ki  (4)  )  ; 
for  i  =  1:4 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' , real (rp_eig (i) ) ,  imag ( rp_eig ( i ) ) ) ; 

end; 

LQR  Height  Controller  M-flle 

%  Z  axis 

vlimith  =  0.1; 

Amh  =  [01 
0  0  ] 

Bmh  =  [0  4 *K/M]  ' ; 

Cmh  =  [1  0 ] ; 

Dmh  =  0 ; 

%  augment  with  integrator 
Aih  =  [Amh  [0  0  ]  ' 

1  0  0]; 

Bih  =  [Bmh '  0 ] ' ; 

Cih  =  eye  (3 ) ; 

Dih  =  [0  0  0]  '  ; 

Q  =  diag ( [ 1  0  50]); 

R  =  5000000; 

kh  =  lqr (Aih, Bih, Q,R); 
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h  eig  =  eig (Aih-Bih*kh) ; 

fprintf  ('************************************************  \n ' )  ; 
fprintf ( ' Z  DESIGN  \n'); 

fprintf (  'P  =  %5.3f  D  =  %5.3f  I  =  %5.3f  \n\n',kh(l),  kh  (2)  ,  kh  (3)  )  ; 
for  i  =  1 :  3 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' , real (h_eig (i) ) ,  imag (h_eig ( i ) ) ) ; 

end; 

Kph  =  kh ( 1 ) ; 

Kdh  =  kh  ( 2  )  ; 

Kwh  =  0 ; 

Kih  =  kh ( 3 ) ; 

LQR  Position  Controller  M-file 

t limit  =  5*pi/180;  %max  pitch  cmd  radians 
%tlimit  =  15*pi/180;  %max  pitch  cmd  radians 
vlimit  =  0.3;  %  max  speed  cmd  in  m/sec 
%vlimit  =  0.5;  %  max  speed  cmd  in  m/sec 

Tau  theta  =  1/7;  %  closed  loop  time  constant  for  pitch  response 
wt  =1/Tau  theta;  %closed  loop  theta  bandwidth 
kt  =  1; 
a  =  [0  1  0  0 
0  0  g  0 
0  0  -wt  0 
1000]; 
b  =  [ 0  0  wt  0  ] ' ; 

q  =  diag ( [  5  2  0  0.1]); 

%q  =  diag([  5  2  0  0.1]); 

%r  =  50; 
r  =  50; 

k  =  lqr (a, b, q, r) ; 

ac  =  a-b*k; 

xy_eig  =  eig(a-b*k); 

Kp  =  k  ( 1 )  ; 

Kd  =  k ( 2 ) ; 

Ki  =  k  ( 4  )  ; 

Kw  =  k  ( 3 )  ; 

fprintf (' \n\n  X  Y  Design  \n'); 

fprintf (  'P  =  %5.3f  D  =  %5.3f  Actuator  =  %5.3f  I  =  %5.3f  \n\n',k(l), 
k (2)  ,  k (3)  ,  k (4) )  ; 
for  i  =  1:4 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' , real (xy_eig (i) ) ,  imag (xy_eig ( i ) ) ) ; 

end; 

LQR  Yaw  Controller  M-flle 

Ky  =  4; 

Jy  =  0.032; 

Amy  =  [01 
0  0]; 

Bmy  =  [0  4*Ky/Jy]'; 
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Cmy  =  eye (2 ) ; 
Dmy  =  [  0 ; 0 ] ; 


Qy  =  diag ( [1  0.1]); 

Ry  =  1000; 

ky  =  lqr (Amy, Bmy, Qy, Ry) ; 
h  eigy  =  eig (Amy-Bmy*ky) ; 

Kpyaw  =  ky  ( 1 ) ; 

Kdyaw  =  ky ( 2 ) ; 

Initialize  Qball  Waypoints.m 

%  clear  all 
close  all 
clc 

height  =  301; 

width  =  301; 

axs  =  [-1  1  -1  1 ] *0.5; 

x  =  [axs  (1)  ,  axs  (2)  ]  ; 
y  =  zeros (1,  length (x) ) ; 

figure ( 1 )  , 

plot (x,  y),hold  on, plot (y,  x)  , 
xlabel ( ' X  (m)  ' ) ,  ylabel ( ' Z  (m)  '  ) 

%  axis ( [-round (width/2)  round (width/2 )  -round (height/2 ) 
round (height/2 ) ] ) 
axis (axs) 

set (gca,  ' Ydir',  'reverse') 
set (gcf ,  'Color',  [1  1  1]); 

Rx  =  0  ; 

Ry  =  0; 

Rt  =0; 

n  =  input ('How  many  waypoints  do  you  want  to  define?  (max 
if  n>10 

n  =  10; 

end 

%  X  and  Y  waypoint  coordinates 
Tx  =  zeros  (11,  1 ) ; 

Tz  =  zeros  (11,  1 )  ; 

%  Mission  state 
%  0  :  do  nothing,  wait 

%  1  :  Goto  waypoint 

%  2  :  Land 

Task  =  ones (11,  1)*2;  %  Set  all  tasks  to  land  initially, 
for  i=l : n 

name  =  strcat (' Enter  the  coordinate  of  waypoint  no.  ' 


10):  '); 


num2str (i) ) 
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title (name) ; 

[tx  tz]  =  ginput(l);  %  in  pix 

plot ( tx,  tz,  's',  ' LineWidth ' ,  2 ,  ' MarkerEdgeColor ' , ' k ' , . . . 

' MarkerFaceColor ' , 'g' , . . . 

'MarkerSize ' , 10) ; 

text(tx+5,  tz+15,  strcat (' Waypoint- ' ,  num2str(i))) 

Tx ( i )  =  tx; 

T  z  ( i )  =  t  z ; 

Task(i)  =  1; 
if  i  ==  n 

Tx (n+1 : 10 )  =  tx; 

Tz (n+1 : 10)  =  tz; 

end 

end 

hold  off 

QBall  Waypoint  State  Machine  function 

function  [h_c,  x_c,  z_c,  s,  i]  =  fcn(x,  z,  y,  Tx,  Tz,  Task,  hgt,  t) 

%  Constants 

WP_TOL  =  0.1;  %  Waypoint  tolerance,  must  be  less  than  this  distance  (m) 
to  arrive  at  waypoint. 

HGT_TOL  =  0.1; 

WP_WAIT  TIME  =  5;  %  Wait  at  waypoint  for  this  many  seconds. 

%  Missions  state 
%  0  :  initialize 

%  1  :  takeoff 

%  2  :  goto  waypoint 

%  3  :  land 

%  4  :  wait  at  waypoint 

persistent  state; 

persistent  wp  index; 
persistent  to_x; 
persistent  to_z; 
persistent  t_start; 
persistent  t_end; 

if  isempty (state) 
state  =  0; 

end 

if  isempty (wp  index) 
wp  index  =  1; 

end 

if  isempty (to_x)  | |  isempty (to_z) 
to_x  =  0; 
to_z  =  0; 

end 

if  isempty (t_start)  | |  isempty (t_end) 
t  start  =  0; 
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t  end  =  0; 


end 

s  =  state; 
i  =  wp  index; 

switch  state 
case  0 

h_c  =  0 ; 
x_c  =  0 ; 
z_c  =  0 ; 
if  t  >=  3 

to_x  =  x; 
to_z  =  z; 
state  =  1; 

end 

case  1 

h_c  =  hgt; 
x_c  =  to_x; 
z_c  =  to_z; 

if  abs (y  -  hgt)  <  HGT_TOL 
state  =  2; 

end 

case  2 

h_c  =  hgt; 
x  c  =  Tx (wp  index); 
z  c  =  Tz (wp  index) ; 
if  Task(wp  index)  ==  1 

if  sqrt ( (x  -  x_c) A2  +  (z  -  z_c)A2)  <  WP_TOL 
state  =  4; 

t_end  =  t  +  WP_WAIT_TIME; 

%wp  index  =  wp  index  +  1; 

end 

else 

to  x  =  Tx (wp  index); 
to  z  =  Tz (wp  index) ; 
state  =  3; 
t_start  =  t; 
t_end  =  t  +  3; 

end 
case  3 

if  t  >=  t  end 
h_c  =  0 ; 

else 

h_c  =  hgt; 

end 

x_c  =  to_x; 
z  c  =  to  z; 


case  4 
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h_c  =  hgt; 
x  c  =  Tx (wp  index); 
z  c  =  Tz (wp  index) ; 
if  t  >=  t  end 
state  =  2; 

wp  index  =  wp  index  +  1; 

end 

otherwise 

state  =  0; 
h_c  =  0 ; 
x_c  =  0 ; 
z_c  =  0 ; 

end 


Initialize  Qbot  Waypoints.m 

close  all 
clc 

load  calibration  file 

height  =  301; 

width  =  301; 

axs  =  [-1  1  -1  1  ]  *  0 . 6 ; 

x  =  [axs  (1)  ,  axs  (2)  ]  ; 
y  =  zeros (1,  length (x) ) ; 

figure ( 1 )  , 

plot (x,  y),hold  on, plot (y,  x)  , 
xlabel ( 'X  (m) ' ) ,  ylabel ( 'Y  (m) ' ) 

%  axis ( [-round (width/2)  round (width/2 )  -round (height/2 ) 
round (height/2  )  ]  ) 
axis (axs) 

Rx  =  0  ; 

Ry  =  0; 

Rt  =0; 

n  =  input ('How  many  waypoints  do  you  want  to  define?  (max 
if  n>10 

n  =  10; 

end 

Tx  =  zeros  (10,  1)  ; 

Ty  =  zeros  ( 10 ,  1 ) ; 
for  i=l : n 

name  =  strcat (' Enter  the  coordinate  of  waypoint  no.  ', 
title (name) ; 

[tx  ty]  =  ginput(l);  %  in  pix 

plot(tx,  ty,  's',  'LineWidth',  2,  ' MarkerEdgeColor ' , ' k 

' MarkerFaceColor ' , ' g ' , . . . 

'MarkerSize ',10); 

text(tx+5,  ty+15,  strcat (' Waypoint- ' ,  num2str(i))) 

Tx ( i )  =  tx; 


10):  '); 


num2str (i)  ) 
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Ty(i)  =  ty; 
if  i  ==  n 

Tx (n+1 : 10 )  =  tx; 

Ty (n+1 : 10)  =  ty; 

end 

end 

hold  off 

Qbot  Mission  Planner  function 

function  [right  vel,  left  vel,  target  distance,  states]  =  ... 
motion  planner (target_list,  robot  xyt,  vlimit,  ... 

err  th,  delta  t,  grid  size,  enc  ang,  ang  thr,  tracking,  pre  states) 

%  Initialize  output  variables 
right  vel  =  intl6(0); 
left_vel  =  intl6(0); 
target_distance  =  -500; 
states  =  pre_states; 

if  ~  (abs  (robot__xyt  ( 1 )  )  ==  5000  |  |  tracking==0) 

%  if  tracking  ==  1 

rx  =  robot_xyt ( 1 ) ; 
ry  =  robot_xyt (2 ) ; 
rtheta  =  robot_xyt ( 3 ) ; 
target_xy  =  [rx  ry]  ; 

[n  xy]  =  size (target_list) ; 
if  n==2  &&  xy==l 

target_xy  =  [targetlist ( 1 , 1 )  target_list (2, 1) ] ; 
n  =  1; 

else 

for  i=l : n 

if  states  ( 1 )  ==  i 
for  j  =1 : xy 

target_xy(j)  =  target_list (i,  j); 

end 

end 

end 

end 

tx  =  targe t_xy ( 1 ) ; 
ty  =  target_xy (2 ) ; 

target_distance  =  f ind_dist ( rx,  ry,  tx,  ty) ; 

if  ( (states (1)  ==  n)  &&  (target_distance  <=  err_th) )  |  |  states  (1) 

if  states  ( 5 )  ==  1 
rtheta  =  0; 

end 

[ang_to_tar,  states (3),  states (4) ]=  f ind_theta (rx,  ry,  rtheta, 
rx+100,  ry,  grid  size,  ... 

enc_ang,  ang_thr,  states (3),  states (4)); 
if  states (4)  ==  1  &&  states (5)  ==  0 

[right  vel,  left  vel]  =  solve  inv  kin(0,  . . . 
ang_to_tar,  vlimit,  delta_t) ; 

else 
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right  vel  =  intl6(0); 
left_vel  =  intl6(0); 
states  ( 5 )  =  1 ; 

end 

else 

if  target  distance  <=  err  th 

states (1)  =  pre_states ( 1 )  +  1; 

else 

[ang_to_tar,  states (3),  states (4) ]=  f ind_theta (rx,  ry, 
rtheta,  tx,  ty,  grid_size,  . . . 

enc_ang,  ang_thr,  states (3),  states (4)); 

[right  vel,  left  vel]  =  solve  inv  kin (target  distance,  . . 
ang_to_tar,  vlimit,  delta_t) ; 

end 

end 

end 

return ; 

o, _ 

o 

function  [y]  =  check  angle (x) 
y  =  x; 
if  x  >  pi 

y  =  x  -  2*pi; 
elseif  x  <  -pi 

y  =  x  +  2*pi; 

end 

return ; 

O,  _ 

o 

function  dist  =  f ind_dist ( rx,  ry,  tx,  ty) 
dist  =  sqrt ( (rx-tx) A2  +  (ry-ty)A2); 

return ; 

O,  _ 

o 

function  [theta,  ang_state,  rotate_state] =  f ind_theta ( rx,  ry,  rtheta, 

tx,  ty,  . . . 

grid_size,  enc_ang,  ang_thr,  pre_ang_state,  pre_rotate_state) 

X  =  roundf (tx  -  rx)/grid  size); 

Y  =  round ((ty  -  ry) /grid_size) ; 
theta  =  atan2 (Y,  X) ; 
if  pre_rotate_state  ==  1 

theta  =  check^angle (theta  -  pre_ang_state) ; 

else 

theta  =  check  angle (theta  -  check  angle ( rtheta) ) ; 

end 

if  abs (theta) >=ang  thr 

if  pre_rotate_state  ==  0 
rotate_state  =  1; 
ang_state  =  rtheta; 

else 

rotate_state  =  pre_rotate_state; 

ang  state  =  check  angle (pre  ang  state  +  enc  ang) ; 

end 

else 

rotate_state  =  0; 
ang_state  =  0; 

end 

return ; 
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delta  t) 


o,  _ 

o 

function  [vr,  vl]  =  solve  inv  kin(dist,  theta, 

d  =  252.5; 

vmax  =  vlimit;%(2); 

wmax  =  (2*vmax)/d; 

w  =  theta/delta_t; 

w  sign  =  sign (w) ; 

if  abs (w)  >  wmax 

w  =  w  sign*wmax; 

vr  =  intl6 (round ( (d*w) / 2) ) ; 

vl  =  intl6(-vr); 

else 

v  =  dist/delta_t; 
vr  tmp  =  (2*v  +  d*w)/2; 
vl  tmp  =  2*v  -  vr  tmp; 

max  of  vrvl  =  abs (max (vr  tmp,  vl  tmp)); 
if  max  of  vrvl  >  vmax 

vr  tmp  =  (vr  tmp/max  of  vrvl) *vmax; 
vl  tmp  =  (vl  tmp/max  of  vrvl) *vmax; 

end 

vr  =  intl6(vr  tmp); 
vl  =  intl6(vl  tmp); 

end 

return ; 

o,  _ 

o 


vlimit. 
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